Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Début des tests avec l'APDS_9960

Browse Source
This commit is contained in:
Samuel 2022-12-22 22:35:49 +01:00
parent 0170af5845
commit 2c4fe4a02e
10 changed files with 987 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
APDS_9960.c
View File

@ -1,10 +1,128 @@
int APDS9960_Init(){
#include "i2c_maitre.h"
#include "pico/stdlib.h"
#include <stdio.h>
#define APDS_ADRESSE 0x39
inline void cc3_rgb2tls (uint8_t rouge, uint8_t vert, uint8_t bleu);
void APDS9960_Init(){
char registre;
char valeur;
i2c_maitre_init();
// Désactivation du module pour le configurer
registre = 0x80;
valeur = 0x0;
while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES);
// Registres à configurer
// ATIME (0x81): temps d'acquisition
// | Valeur | Nb cycles | Temps |
// | 0 | 255 | 712 ms |
// | 182 | 72 | 200 ms |
// | 255 | 1 | 2,78 ms |
registre = 0x81;
valeur = 220;
while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES);
// CONTROL<AGAIN> (0x8F<1:0>): Gain
// 0 : x1
// 1 : x4
// 2 : x16
// 3 : x64
registre = 0x8F;
valeur = 0x03;
while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES);
// Temps d'attente : Wen, WTIME, WLONG
// ENABLE (PON) Address : 0x80 - Après les autres registres !!
// RESERVED | Gesture En | Prox. Interrupt En | ALS Interrupt En | Wait En | Proximity Detect En | ALS En | Power On |
// Ob0000 0011 => 0x03
registre = 0x80;
valeur = 0x3;
while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES);
}
int APDS9960_Lire(){
unsigned char reception[8];
uint16_t clear, rouge, vert, bleu;
i2c_lire_registre(APDS_ADRESSE, 0x94, reception, 8);
clear = (uint16_t) reception[0] + (uint16_t) reception[1] << 8;
rouge = (uint16_t) reception[2] + (uint16_t) reception[3] << 8;
vert = (uint16_t) reception[4] + (uint16_t) reception[5] << 8;
bleu = (uint16_t) reception[6] + (uint16_t) reception[7] << 8;
printf("clear:%u\n", clear/256);
printf("rouge:%u\n", rouge/256);
printf("vert:%u\n", vert/256);
printf("bleu:%u\n", bleu/256);
cc3_rgb2tls(rouge/256, vert/256, bleu/256);
return 1;
}
inline void cc3_rgb2tls (uint8_t rouge, uint8_t vert, uint8_t bleu)
{
uint8_t hue, sat, val;
uint8_t rgb_min, rgb_max;
rgb_max = rouge;
rgb_min = rouge;
if (rouge > rgb_max)
rgb_max = rouge;
if (vert > rgb_max)
rgb_max = vert;
if (bleu > rgb_max)
rgb_max = bleu;
if (rouge < rgb_min)
rgb_min = rouge;
if (vert < rgb_min)
rgb_min = vert;
if (bleu < rgb_min)
rgb_min = bleu;
// compute V
val = rgb_max;
if (val == 0) {
printf("val:0");
hue = sat = 0;
return;
}
// compute S
sat = 255 * (rgb_max - rgb_min) / val;
if (sat == 0) {
printf("sat:0");
return;
}
// compute H
if (rgb_max == rouge) {
hue =
0 + 43 * (int16_t)(vert -
bleu) / (rgb_max - rgb_min);
}
else if (rgb_max == vert) {
hue =
85 + 43 * (int16_t)(bleu -
rouge) / (rgb_max - rgb_min);
}
else { /* rgb_max == blue */
hue =
171 + 43 * (int16_t)(rouge -
vert) / (rgb_max - rgb_min);
}
printf("val:%u\n", val);
printf("sat:%u\n", sat);
printf("teinte:%u\n", hue*360/250);
}

2
APDS_9960.h
View File

@ -0,0 +1,2 @@
void APDS9960_Init(void);
int APDS9960_Lire(void);

5
CMakeLists.txt
View File

@ -17,6 +17,7 @@ QEI.c
gyro.c
gyro_L3GD20H.c
gyro_ADXRS453.c
i2c_maitre.c
Localisation.c
Moteurs.c
Robot_config.c
@ -36,9 +37,9 @@ add_definitions(-DGYRO_ADXRS453)
pico_enable_stdio_usb(test 1)
pico_enable_stdio_uart(test 1)
pico_add_extra_outputs(test)
target_link_libraries(test pico_stdlib hardware_timer hardware_spi hardware_pwm hardware_structs hardware_pio pico_multicore)
target_link_libraries(test pico_stdlib hardware_i2c hardware_timer hardware_spi hardware_pwm hardware_structs hardware_pio pico_multicore)
add_custom_target(Flash
DEPENDS test
COMMAND sudo picotool load ${PROJECT_BINARY_DIR}/test.uf2
COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/test.uf2
)

129
Holonome2023.c Normal file
View File

@ -0,0 +1,129 @@
#include <stdio.h>
#include "pico/multicore.h"
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/timer.h"
#include "pico/binary_info.h"
#include "math.h"
#include "Test.h"
#include "gyro.h"
#include "Asser_Moteurs.h"
#include "Asser_Position.h"
#include "Commande_vitesse.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "QEI.h"
#include "Servomoteur.h"
#include "spi_nb.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
const uint LED_PIN = 25;
const uint LED_PIN_ROUGE = 28;
const uint LED_PIN_NE_PAS_UTILISER = 22;
uint temps_cycle;
#define V_INIT -999.0
#define TEST_TIMEOUT_US 10000000
int mode_test();
int main() {
bi_decl(bi_program_description("This is a test binary."));
bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED"));
double vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT;
struct t_angle_gyro_double angle_gyro;
uint32_t temps_ms = 0, temps_ms_old;
uint32_t temps_us_debut_cycle;
stdio_init_all();
gpio_init(LED_PIN);
gpio_set_dir(LED_PIN, GPIO_OUT);
gpio_put(LED_PIN, 1);
gpio_init(LED_PIN_ROUGE);
gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT);
gpio_put(LED_PIN_ROUGE, 1);
// Il faut neutraliser cette broche qui pourrait interférer avec
// la lecture des codeurs. (problème sur la carte électrique)...
gpio_init(LED_PIN_NE_PAS_UTILISER);
gpio_set_dir(LED_PIN_NE_PAS_UTILISER, GPIO_IN);
sleep_ms(3000);
Servomoteur_Init();
//puts("Debut");
//spi_test();
//while(1);
Temps_init();
Moteur_Init();
QEI_init();
AsserMoteur_Init();
Localisation_init();
while(mode_test());
Gyro_Init();
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
while (1) {
u_int16_t step_ms = 2;
float coef_filtre = 1-0.8;
while(temps_ms == Temps_get_temps_ms());
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
// Tous les pas de step_ms
if(!(temps_ms % step_ms)){
temps_us_debut_cycle = time_us_32();
Gyro_Read(step_ms);
//gyro_affiche(gyro_get_vitesse(), "Angle :");
// Filtre
angle_gyro = gyro_get_vitesse();
if(vitesse_filtre_x == V_INIT){
vitesse_filtre_x = angle_gyro.rot_x;
vitesse_filtre_y = angle_gyro.rot_y;
vitesse_filtre_z = angle_gyro.rot_z;
}else{
vitesse_filtre_x = vitesse_filtre_x * (1-coef_filtre) + angle_gyro.rot_x * coef_filtre;
vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre;
vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre;
}
temps_cycle = time_us_32() - temps_us_debut_cycle;
//printf("%#x, %#x\n", (double)temps_ms_old / 1000, vitesse_filtre_z);
//printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000));
//gyro_affiche(angle_gyro, "Vitesse (°/s),");
}
// Toutes les 50 ms
if((Temps_get_temps_ms() % 50) == 0){
struct t_angle_gyro_double m_gyro;
m_gyro = gyro_get_angle_degres();
printf("%f, %f, %d\n", (double)temps_ms / 1000, m_gyro.rot_z, temps_cycle);
}
// Toutes les 500 ms
if((Temps_get_temps_ms() % 500) == 0){
//gyro_affiche(gyro_get_angle(), "Angle :");
}
// Toutes les secondes
if((Temps_get_temps_ms() % 500) == 0){
//gyro_get_temp();
}
}
}

9
Robot_config.c Normal file
View File

@ -0,0 +1,9 @@
int position_avec_gyroscope = 0;
int get_position_avec_gyroscope(void){
return position_avec_gyroscope;
}
void set_position_avec_gyroscope(int _use_gyro){
position_avec_gyroscope = _use_gyro;
}

2
Robot_config.h Normal file
View File

@ -0,0 +1,2 @@
int get_position_avec_gyroscope(void);
void set_position_avec_gyroscope(int _use_gyro);

652
Test.c Normal file
View File

@ -0,0 +1,652 @@
#include <stdio.h>
#include "pico/multicore.h"
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "pico/binary_info.h"
#include "math.h"
#include "Test.h"
#include "APDS_9960.h"
#include "gyro.h"
#include "Asser_Moteurs.h"
#include "Asser_Position.h"
#include "Commande_vitesse.h"
#include "i2c_maitre.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Servomoteur.h"
#include "spi_nb.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#define V_INIT -999.0
#define TEST_TIMEOUT_US 10000000
int test_APDS9960(void);
int test_moteurs(void);
int test_QIE(void);
int test_QIE_mm(void);
int test_vitesse_moteur(enum t_moteur moteur);
int test_asser_moteur(void);
int test_localisation(void);
int test_avance(void);
int test_cde_vitesse_rotation(void);
int test_cde_vitesse_rectangle(void);
int test_cde_vitesse_cercle(void);
int test_asser_position_avance(void);
int test_asser_position_avance_et_tourne(int);
int test_trajectoire(void);
int test_tca9535(void);
void affiche_localisation(void);
// Mode test : renvoie 0 pour quitter le mode test
int mode_test(){
static int iteration = 2;
printf("Appuyez sur une touche pour entrer en mode test :\n");
printf("A - pour asser_moteurs (rotation)\n");
printf("B - pour avance (asser_moteur)\n");
printf("C - pour les codeurs\n");
printf("D - pour les codeurs (somme en mm)\n");
printf("E - Commande en vitesse - rotation pure\n");
printf("F - Commande en vitesse - carré\n");
printf("G - Commande en vitesse - cercle\n");
printf("H - Asser Position - avance\n");
printf("I - Asser Position - avance et tourne (gyro)\n");
printf("J - Asser Position - avance et tourne (sans gyro)\n");
printf("M - pour les moteurs\n");
printf("L - pour la localisation\n");
printf("T - Trajectoire\n");
printf("U - Scan du bus i2c\n");
printf("V - APDS_9960\n");
stdio_flush();
int rep = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
switch (rep)
{
case 'a':
case 'A':
while(test_asser_moteur());
break;
case 'b':
case 'B':
while(test_avance());
break;
case 'C':
case 'c':
while(test_QIE());
break;
case 'D':
case 'd':
while(test_QIE_mm());
break;
case 'E':
case 'e':
while(test_cde_vitesse_rotation());
break;
case 'F':
case 'f':
while(test_cde_vitesse_rectangle());
break;
case 'G':
case 'g':
while(test_cde_vitesse_cercle());
break;
case 'H':
case 'h':
while(test_asser_position_avance());
break;
case 'I':
case 'i':
while(test_asser_position_avance_et_tourne(1));
break;
case 'J':
case 'j':
while(test_asser_position_avance_et_tourne(0));
break;
case 'M':
case 'm':
while(test_moteurs());
break;
case 'L':
case 'l':
while(test_localisation());
break;
case 'T':
case 't':
while(test_trajectoire());
break;
case 'U':
case 'u':
while(test_tca9535());
break;
case 'V':
case 'v':
while(test_APDS9960());
break;
case PICO_ERROR_TIMEOUT:
iteration--;
if(iteration == 0){
printf("Sortie du mode test\n");
return 0;
}
default:
printf("Commande inconnue\n");
break;
}
return 1;
}
bool reserved_addr(uint8_t addr) {
return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
}
int test_APDS9960(){
int lettre;
printf("Initialisation\n");
APDS9960_Init();
printf("Lecture...\n");
/*
do{
APDS9960_Lire();
lettre = getchar_timeout_us(0);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);*/
while(1){
APDS9960_Lire();
sleep_ms(100);
}
return 1;
}
int test_tca9535(){
// Adresse I2C : 0b0100 000 R/W
// Lecture des broches sur les registres 0 et 1
// Registre 2 et 3 : valeur des broches en sorties
// Registre 4 et 5 : INversion de polarité
// Registre 6 et 7 : Configuration entrée (1) ou sortie (0)
uint8_t reception[8];
uint8_t emission[8];
//uint8_t adresse = 0b0100000;
uint8_t adresse = 0x39;
int statu;
int lettre;
emission[0]=6; // Registre à lire
i2c_maitre_init();
// Scan bus I2C - cf SDK
printf("\nI2C Bus Scan\n");
printf(" 0 1 2 3 4 5 6 7 8 9 A B C D E F\n");
for (int addr = 0; addr < (1 << 7); ++addr) {
if (addr % 16 == 0) {
printf("%02x ", addr);
}
int ret;
uint8_t rxdata;
if (reserved_addr(addr))
ret = PICO_ERROR_GENERIC;
else
ret = i2c_read_blocking(i2c_default, addr, &rxdata, 1, false);
printf(ret < 0 ? "." : "@");
printf(addr % 16 == 15 ? "\n" : " ");
}
printf("Done.\n");
return 0;
do{
statu = i2c_write_blocking (i2c0, adresse, emission, 1, 0);
if(statu == PICO_ERROR_GENERIC){
printf("Emission : Address not acknowledged, no device present.\n");
return 0;
}else{
printf("Emission : Ok\n");
}
statu = i2c_read_blocking(i2c0, adresse, reception, 2, 0);
if(statu == PICO_ERROR_GENERIC){
printf("Reception : Address not acknowledged, no device present.\n");
return 0;
}else{
printf("Recetion : Ok\n");
}
printf("%2.x%2.x\n",reception[0], reception[1]);
lettre = getchar_timeout_us(0);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
void test_trajectoire_printf(){
struct position_t _position;
while(1){
_position = Trajet_get_consigne();
printf("T: %ld, X: %f, Y: %f, orientation: %2.1f\n", time_us_32()/1000, _position.x_mm, _position.y_mm, _position.angle_radian/M_PI*180);
}
}
int test_trajectoire(){
int lettre, _step_ms = 1, temps_ms=0;
Trajet_init();
struct trajectoire_t trajectoire;
printf("Choix trajectoire :\n");
printf("B - Bezier\n");
printf("C - Circulaire\n");
printf("D - Droite\n");
do{
lettre = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
switch(lettre){
case 'b':
case 'B':
Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0);
break;
case 'c':
case 'C':
Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250);
break;
case 'd':
case 'D':
Trajectoire_droite(&trajectoire, 0, 0, 0, 700);
break;
default: return 0;
}
Trajet_debut_trajectoire(trajectoire);
multicore_launch_core1(test_trajectoire_printf);
do{
// Routines à 1 ms
QEI_update();
Localisation_gestion();
if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
Moteur_SetVitesse(MOTEUR_C, 0);
}else{
AsserMoteur_Gestion(_step_ms);
}
sleep_ms(_step_ms);
temps_ms += _step_ms;
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
/// @return
int test_asser_position_avance_et_tourne(int m_gyro){
int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2;
uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old;
struct position_t position_consigne;
position_consigne.angle_radian = 0;
position_consigne.x_mm = 0;
position_consigne.y_mm = 0;
printf("Le robot avance à 100 mm/s\n");
printf("Init gyroscope\n");
Gyro_Init();
printf("C'est parti !\n");
stdio_flush();
set_position_avec_gyroscope(m_gyro);
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
temps_ms_init = temps_ms;
multicore_launch_core1(affiche_localisation);
do{
while(temps_ms == Temps_get_temps_ms());
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
QEI_update();
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
position_consigne.angle_radian = (double) (temps_ms - temps_ms_init) /1000.;
position_consigne.y_mm = (double) (temps_ms - temps_ms_init) * 100. / 1000.;
Asser_Position(position_consigne);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
printf("lettre : %c, %d\n", lettre, lettre);
return 0;
}
int test_asser_position_avance(){
int lettre, _step_ms = 1, temps_ms=0;
struct position_t position;
position.angle_radian = 0;
position.x_mm = 0;
position.y_mm = 0;
printf("Le robot avance à 100 mm/s\n");
do{
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
if(temps_ms < 5000){
position.y_mm = (double) temps_ms * 100. / 1000.;
}else if(temps_ms < 10000){
position.y_mm = 1000 - (double) temps_ms * 100. / 1000.;
}else{
temps_ms = 0;
}
Asser_Position(position);
temps_ms += _step_ms;
sleep_ms(_step_ms);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_cde_vitesse_rotation(){
int lettre, _step_ms = 1;
double vitesse =90.0/2 * 3.14159 /180.0;
printf("Rotation du robot sur lui-même en 8 secondes\nVitesse : %f rad/s\n", vitesse);
commande_vitesse(0, 0, vitesse);
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
sleep_ms(_step_ms);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_cde_vitesse_rectangle(){
int lettre, _step_ms = 1, temps_ms=0;
printf("déplacement en rectangle du robot : 500x200 mm, 100 mm/s\n");
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
if(temps_ms < 5000){
commande_vitesse(0, 100, 0);
}else if(temps_ms < 7000){
commande_vitesse(-100, 0, 0);
}else if(temps_ms < 12000){
commande_vitesse(0, -100, 0);
}else if(temps_ms < 14000){
commande_vitesse(100, 0, 0);
}else{
temps_ms = 0;
}
sleep_ms(_step_ms);
temps_ms += _step_ms;
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_cde_vitesse_cercle(){
int lettre, _step_ms = 1, temps_ms=0;
printf("déplacement en cercle du robot : 100 mm/s\n");
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
commande_vitesse(cos((double)temps_ms / 1000.) * 200.0, sin((double)temps_ms /1000.) * 200.0, 0);
temps_ms += _step_ms;
sleep_ms(_step_ms);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_avance(void){
int lettre;
int _step_ms = 1;
AsserMoteur_setConsigne_mm_s(MOTEUR_A, -100);
AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0);
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
sleep_ms(_step_ms);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
Moteur_SetVitesse(MOTEUR_C, 0);
return 0;
}
void affiche_localisation(){
struct position_t position;
while(1){
position = Localisation_get();
printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
}
}
void test_asser_moteur_printf(){
int _step_ms = 1;
while(1){
printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
//sleep_ms(5);
}
}
int test_asser_moteur(){
int lettre;
int _step_ms = 1;
printf("Asservissement des moteurs :\nAppuyez sur une touche pour quitter\n");
AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100);
AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
AsserMoteur_setConsigne_mm_s(MOTEUR_C, 100);
multicore_launch_core1(test_asser_moteur_printf);
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
sleep_ms(_step_ms);
//printf("Vitesse A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
//printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
// AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
Moteur_SetVitesse(MOTEUR_C, 0);
multicore_reset_core1();
return 0;
}
int test_QIE(){
int lettre;
printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
do{
QEI_update();
printf("Codeur A : %d (%3.2f mm), codeur B : %d (%3.2f mm), codeur C : %d (%3.2f mm)\n",
QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME),
QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME),
QEI_get(QEI_C_NAME), QEI_get_mm(QEI_C_NAME));
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_QIE_mm(){
int lettre;
printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
double a_mm=0, b_mm=0, c_mm=0;
do{
QEI_update();
a_mm += QEI_get_mm(QEI_A_NAME);
b_mm += QEI_get_mm(QEI_B_NAME);
c_mm += QEI_get_mm(QEI_C_NAME);
printf("Codeur A : %3.2f mm, codeur B : %3.2f mm, codeur C : %3.2f mm\n", a_mm, b_mm, c_mm);
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_localisation(){
int lettre;
struct position_t position;
printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
do{
QEI_update();
Localisation_gestion();
position = Localisation_get();
printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_moteurs(){
int lettre_moteur;
printf("Indiquez le moteurs à tester (A, B ou C):\n");
do{
lettre_moteur = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre_moteur == PICO_ERROR_TIMEOUT);
printf("Moteur choisi : %c %d %x\n", lettre_moteur, lettre_moteur, lettre_moteur);
switch (lettre_moteur)
{
case 'A':
case 'a':
while(test_vitesse_moteur(MOTEUR_A));
break;
case 'B':
case 'b':
while(test_vitesse_moteur(MOTEUR_B));
break;
case 'C':
case 'c':
while(test_vitesse_moteur(MOTEUR_C));
break;
case 'Q':
case 'q':
return 0;
break;
default:
break;
}
return 1;
}
int test_vitesse_moteur(enum t_moteur moteur){
printf("Vitesse souhaitée :\n0 - 0%%\n1 - 10%%\n2 - 20%%\n...\n9 - 90%%\nA - 100%%\n");
int vitesse_moteur;
do{
vitesse_moteur = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(vitesse_moteur == PICO_ERROR_TIMEOUT);
switch (vitesse_moteur)
{
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
printf("Vitesse choisie : %c0%%\n", vitesse_moteur);
Moteur_SetVitesse(moteur, (vitesse_moteur - '0') * 32767.0 / 10.);
break;
case 'A':
case 'a':
printf("Vitesse choisie : 100%%\n");
Moteur_SetVitesse(moteur, (int16_t) 32766.0);
break;
case 'b':
case 'B':
printf("Vitesse choisie : -50%%\n");
Moteur_SetVitesse(moteur, (int16_t) -32766.0/2);
break;
case 'q':
case 'Q':
return 0;
break;
default:
break;
}
return 1;
}

1
Test.h Normal file
View File

@ -0,0 +1 @@
int mode_test();

61
i2c_maitre.c Normal file
View File

@ -0,0 +1,61 @@
#include "i2c_maitre.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "pico/stdlib.h"
#include <stdio.h>
#define I2C_SDA_PIN 16
#define I2C_SCL_PIN 17
void i2c_maitre_init(void){
stdio_init_all();
i2c_init(i2c0, 100 * 1000);
gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
gpio_set_function(6, GPIO_FUNC_NULL);
gpio_set_function(7, GPIO_FUNC_NULL);
}
/// @brief Pour l'instant bloquant, mais devrait passer en non bloquant bientôt
/// @param adresse_7_bits
/// @param
/// @return 0: en cours,
int i2c_lire_registre(char adresse_7_bits, char registre, char * reception, char len){
int statu;
char emission[1];
emission[0] = registre;
statu = i2c_write_blocking (i2c0, adresse_7_bits, emission, 1, 0);
if(statu == PICO_ERROR_GENERIC){
printf("I2C - Envoi registre Echec\n");
return I2C_ECHEC;
}
statu = i2c_read_blocking (i2c0, adresse_7_bits, reception, len, 0);
if(statu == PICO_ERROR_GENERIC){
printf("I2C - Lecture registre Echec\n");
return I2C_ECHEC;
}
return I2C_SUCCES;
}
int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre){
int statu;
char emission[2];
emission[0] = registre;
emission[1] = valeur_registre;
statu = i2c_write_blocking (i2c0, adresse_7_bits, emission, 2, 0);
if(statu == PICO_ERROR_GENERIC){
printf("Erreur ecrire registre\n");
return I2C_ECHEC;
}
printf("i2c Registre %x, valeur %x\n", registre, valeur_registre);
return I2C_SUCCES;
}

9
i2c_maitre.h Normal file
View File

@ -0,0 +1,9 @@
#define I2C_EN_COURS 0
#define I2C_SUCCES 1
#define I2C_ECHEC -1
void i2c_maitre_init(void);
int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre);
int i2c_lire_registre(char adresse_7_bits, char registre, char * reception, char len);