pfrison / projet-crep

Commit 3fcb1d7bf4f287e3324e2ea669c1bc7c64077fe6

Authored by pfrison
1 parent d2f14e3c

update robot code for PercTeacher

Showing 1 changed file with 36 additions and 22 deletions   Show diff stats
Inline Side-by-side
PercTeacher/Sources/Robot/robot.c
  Show/Hide comments   View file @3fcb1d7
@@ -3,29 +3,42 @@ @@ -3,29 +3,42 @@
3 3
4 #define SERIAL_BAUDE_RATE 9600 4 #define SERIAL_BAUDE_RATE 9600
5 #define DELAI_STEP 2 5 #define DELAI_STEP 2
6 -#define PIN_MOTEUR_GAUCHE 0x40 // pin 12  
7 -#define PIN_MOTEUR_DROIT 0x20 // pin 11 6 +#define STEP_MOTEUR_GAUCHE 0x00 // pin ??
  7 +#define STEP_MOTEUR_DROIT 0x00 // pin ??
  8 +#define DIR_MOTEUR_GAUCHE 0x00 // pin ??
  9 +#define DIR_MOTEUR_DROIT 0x00 // pin ??
8 10
9 #define CPU_FREQ 16000000L 11 #define CPU_FREQ 16000000L
10 12
11 //gestion des moteurs 13 //gestion des moteurs
12 void init_pins(void){ 14 void init_pins(void){
13 - DDRB = PIN_MOTEUR_GAUCHE | PIN_MOTEUR_DROIT; 15 + DDRB = STEP_MOTEUR_GAUCHE | STEP_MOTEUR_DROIT | DIR_MOTEUR_GAUCHE | DIR_MOTEUR_DROIT;
14 } 16 }
15 17
16 void pas_moteur(int g, int d){ 18 void pas_moteur(int g, int d){
17 - if (g)  
18 - PORTB |= PIN_MOTEUR_GAUCHE;  
19 - if (d)  
20 - PORTB |= PIN_MOTEUR_DROIT; 19 + if (g != 0)
  20 + PORTB |= STEP_MOTEUR_GAUCHE;
  21 + if (d != 0)
  22 + PORTB |= STEP_MOTEUR_DROIT;
21 _delay_ms(DELAI_STEP); 23 _delay_ms(DELAI_STEP);
22 - if (g)  
23 - PORTB &= ~PIN_MOTEUR_GAUCHE;  
24 - if (d)  
25 - PORTB &= ~PIN_MOTEUR_DROIT; 24 + if (g != 0)
  25 + PORTB &= ~STEP_MOTEUR_GAUCHE;
  26 + if (d != 0)
  27 + PORTB &= ~STEP_MOTEUR_DROIT;
26 _delay_ms(DELAI_STEP); 28 _delay_ms(DELAI_STEP);
27 } 29 }
28 30
  31 +void moteur_set_dir(int g, int d){
  32 + if(g != 0)
  33 + PORTB |= DIR_MOTEUR_GAUCHE;
  34 + else
  35 + PORTB &= ~DIR_MOTEUR_GAUCHE;
  36 + if(d != 0)
  37 + PORTB |= DIR_MOTEUR_DROIT;
  38 + else
  39 + PORTB &= ~DIR_MOTEUR_DROIT;
  40 +}
  41 +
29 //gestion de la liaison serie 42 //gestion de la liaison serie
30 void init_serial(int speed){ 43 void init_serial(int speed){
31 UBRR0 = CPU_FREQ / (((unsigned long int) speed) << 4) - 1; //Set baud rate 44 UBRR0 = CPU_FREQ / (((unsigned long int) speed) << 4) - 1; //Set baud rate
@@ -53,16 +66,18 @@ int main(void){ @@ -53,16 +66,18 @@ int main(void){
53 unsigned char deltaL2 = get_serial(); 66 unsigned char deltaL2 = get_serial();
54 unsigned char deltaR1 = get_serial(); 67 unsigned char deltaR1 = get_serial();
55 unsigned char deltaR2 = get_serial(); 68 unsigned char deltaR2 = get_serial();
56 -  
57 -  
58 - send_serial('a');  
59 - 69 +
60 // conversion 2x8 bits => 16 bits 70 // conversion 2x8 bits => 16 bits
61 - int deltaL = (deltaL1 << 8) + deltaL2;  
62 - int deltaR = (deltaR1 << 8) + deltaR2; 71 + uint16_t deltaL = (deltaL1 << 8) + deltaL2;
  72 + uint16_t deltaR = (deltaR1 << 8) + deltaR2;
  73 +
  74 + // sens rotation
  75 + moteur_set_dir((deltaL & 0x80) == 0x80 ? 1 : 0, (deltaR & 0x80) == 0x80 ? 1 : 0)
  76 + deltaL &= 0x7F;
  77 + deltaR &= 0x7F;
63 78
64 - int deltaMax = deltaL > deltaR ? deltaL : deltaR;  
65 - for(int i=0; i<deltaMax; i++){ 79 + uint16_t deltaMax = deltaL > deltaR ? deltaL : deltaR;
  80 + for(uint16_t i=0; i<deltaMax; i++){
66 if(deltaL > i && deltaR > i) 81 if(deltaL > i && deltaR > i)
67 pas_moteur(1, 1); 82 pas_moteur(1, 1);
68 else if(deltaL > i) 83 else if(deltaL > i)
@@ -70,9 +85,8 @@ int main(void){ @@ -70,9 +85,8 @@ int main(void){
70 else if(deltaR > i) 85 else if(deltaR > i)
71 pas_moteur(0, 1); 86 pas_moteur(0, 1);
72 } 87 }
73 -  
74 - send_serial('b');  
75 - send_serial(0x01); 88 +
  89 + send_serial(0x01);
76 } 90 }
77 return 0; 91 return 0;
78 } 92 }