Bump detektor för robotar med aluminiumfolie (4 / 4 steg)
Steg 4: kod
Vi ville ha våra robo gå bakåt och sedan höger när den stötte. Så det här är koden:
(Skriven för Arduino)
#define mota1 4
#define mota2 53
#define motb1 7
#define motb2 6
#define en1 3
#define en2 5
void forward(void);
void reverse2(void);
void pause(void);
void right(void);
void setup()
{
Serial.BEGIN(9600);
pinMode(19,INPUT);
attachInterrupt(4,bump,LOW);
pinMode(mota1,OUTPUT);
pinMode(mota2,OUTPUT);
pinMode(motb1,OUTPUT);
pinMode(motb2,OUTPUT);
pinMode(en1,OUTPUT);
pinMode(en2,OUTPUT);
}
void loop()
{
Forward();
}
void bump()
{
för (länge jag = 0; jag < 10000; i ++) {
PAUSE();
}
för (länge jag = 0; jag < 30000; i ++) {
reverse2();
}
för (länge jag = 0; jag < 60000; i ++) {
Right();
}
}
void forward()
{
analogWrite(en1,125);
analogWrite(en2,125);
digitalWrite(mota1,HIGH);
digitalWrite(mota2,LOW);
digitalWrite(motb1,HIGH);
digitalWrite(motb2,LOW);
}
void reverse2(void) {
analogWrite(en1,255);
analogWrite(en2,255);
digitalWrite(mota1,LOW);
digitalWrite(mota2,HIGH);
digitalWrite(motb1,LOW);
digitalWrite(motb2,HIGH);
}
void right(void)
{
analogWrite(en1,0);
analogWrite(en2,255);
digitalWrite(mota1,HIGH);
digitalWrite(mota2,LOW);
digitalWrite(motb1,HIGH);
digitalWrite(motb2,LOW);
}
void pause()
{
analogWrite(en1,0);
analogWrite(en2,0);
}