servoAndStepperControl.c
1.89 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
#include <avr/io.h>
#include <util/delay.h>
#define SERIAL_BAUDE_RATE 9600
#define DELAI_STEP 2
#define PIN_MOTEUR_GAUCHE 0x40 // pin 12
#define PIN_MOTEUR_DROIT 0x20 // pin 11
#define CPU_FREQ 16000000L
//gestion des moteurs
void init_pins(void){
DDRB = PIN_MOTEUR_GAUCHE | PIN_MOTEUR_DROIT;
}
void pas_moteur(int g, int d){
if (g)
PORTB |= PIN_MOTEUR_GAUCHE;
if (d)
PORTB |= PIN_MOTEUR_DROIT;
_delay_ms(DELAI_STEP);
if (g)
PORTB &= ~PIN_MOTEUR_GAUCHE;
if (d)
PORTB &= ~PIN_MOTEUR_DROIT;
_delay_ms(DELAI_STEP);
}
//gestion de la liaison serie
void init_serial(int speed){
UBRR0 = CPU_FREQ / (((unsigned long int) speed) << 4) - 1; //Set baud rate
UCSR0B = (1 << TXEN0 | 1 << RXEN0); //Enable transmitter & receiver
UCSR0C = (1 << UCSZ01 | 1 << UCSZ00); //Set 8 bits character and 1 stop bit
UCSR0A &= ~(1 << U2X0); //Set off UART baud doubler
}
void send_serial(unsigned char c){
loop_until_bit_is_set(UCSR0A, UDRE0);
UDR0 = c;
}
unsigned char get_serial(void){
loop_until_bit_is_set(UCSR0A, RXC0);
return UDR0;
}
int main(void){
init_pins();
init_serial(SERIAL_BAUDE_RATE);
while(1){
unsigned char deltaL1 = get_serial();
unsigned char deltaL2 = get_serial();
unsigned char deltaR1 = get_serial();
unsigned char deltaR2 = get_serial();
send_serial('a');
// conversion 2x8 bits => 16 bits
int deltaL = (deltaL1 << 8) + deltaL2;
int deltaR = (deltaR1 << 8) + deltaR2;
int deltaMax = deltaL > deltaR ? deltaL : deltaR;
for(int i=0; i<deltaMax; i++){
if(deltaL > i && deltaR > i)
pas_moteur(1, 1);
else if(deltaL > i)
pas_moteur(1, 0);
else if(deltaR > i)
pas_moteur(0, 1);
}
send_serial('b');
send_serial(0x01);
}
return 0;
}