Commit 3fcb1d7bf4f287e3324e2ea669c1bc7c64077fe6
1 parent
d2f14e3c
update robot code for PercTeacher
Showing
1 changed file
with
36 additions
and
22 deletions
Show diff stats
PercTeacher/Sources/Robot/robot.c
| ... | ... | @@ -3,29 +3,42 @@ |
| 3 | 3 | |
| 4 | 4 | #define SERIAL_BAUDE_RATE 9600 |
| 5 | 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 | 11 | #define CPU_FREQ 16000000L |
| 10 | 12 | |
| 11 | 13 | //gestion des moteurs |
| 12 | 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 | 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 | 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 | 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 | 42 | //gestion de la liaison serie |
| 30 | 43 | void init_serial(int speed){ |
| 31 | 44 | UBRR0 = CPU_FREQ / (((unsigned long int) speed) << 4) - 1; //Set baud rate |
| ... | ... | @@ -53,16 +66,18 @@ int main(void){ |
| 53 | 66 | unsigned char deltaL2 = get_serial(); |
| 54 | 67 | unsigned char deltaR1 = get_serial(); |
| 55 | 68 | unsigned char deltaR2 = get_serial(); |
| 56 | - | |
| 57 | - | |
| 58 | - send_serial('a'); | |
| 59 | - | |
| 69 | + | |
| 60 | 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 | 81 | if(deltaL > i && deltaR > i) |
| 67 | 82 | pas_moteur(1, 1); |
| 68 | 83 | else if(deltaL > i) |
| ... | ... | @@ -70,9 +85,8 @@ int main(void){ |
| 70 | 85 | else if(deltaR > i) |
| 71 | 86 | pas_moteur(0, 1); |
| 72 | 87 | } |
| 73 | - | |
| 74 | - send_serial('b'); | |
| 75 | - send_serial(0x01); | |
| 88 | + | |
| 89 | + send_serial(0x01); | |
| 76 | 90 | } |
| 77 | 91 | return 0; |
| 78 | 92 | } | ... | ... |