SwarmBox/BoxBot.pde at master · tub/SwarmBox · GitHub

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
#include <Servo.h>

int leftPin = 2;
int rightPin = 3;

Servo left;  // create servo object to control a servo
Servo right;  // create servo object to control a servo

boolean leftWhisker = false;
boolean rightWhisker = false;

void setup()
{
  left.attach(9);
  right.attach(10);
  attachInterrupt(0, bumpLeft, CHANGE);
  attachInterrupt(1, bumpRight, CHANGE);
}


void loop()
{
  if(leftWhisker && rightWhisker){
    go(-90);
  }else if(leftWhisker || rightWhisker){
    if(leftWhisker){
      turnRight();
    }else{
      turnLeft();
    }
    delay(random(100,1000));
  }else{
    go(90);
  }
}

void bumpLeft(){
  leftWhisker = !digitalRead(leftPin);
}

void bumpRight(){
  rightWhisker = !digitalRead(rightPin);
}

void go(int wheelSpeed){
  int leftSpeed = 90 + wheelSpeed;
  int rightSpeed = 90 - wheelSpeed;
  left.write(leftSpeed);
  right.write(rightSpeed);
}

void forward(){
  go(-10);
}

void turnRight(){
  left.write(120);
  right.write(120);
}

void turnLeft(){
  left.write(50);
  right.write(50);
}

void wheelStop(){
  left.write(90);
  right.write(90);
}