Tapeworm

gistfile1.ino

#define AUTO 0
#define MANUAL 1

#define trigPin 13
#define echoPin 12

#define b1 4 
#define b2 5
#define a2 6
#define a1 7

/*
x1 x2 function
 0   0 coast/fastdecay
 0   1 reverse
 1   0 forward
 1   1 brake/slow decay
 
 
 B1 3
 B2 2
 A2 1 
 A1 0
 
 COMMANDS:
 
 4: off
 1: on
 2: forward
 3: reverse
 */

int const MINDIST = 40;
int const MAXDIST = 100;
int const FORWARD = 1;
int const REVERSE = 0;
int const sr04Delay = 100;
int twist_direction = 1;
long distance;
boolean on = true;
boolean runAuto = true;

void setup() {
  pinMode(b1, OUTPUT);
  pinMode(b2, OUTPUT);
  pinMode(a2, OUTPUT);
  pinMode(a1, OUTPUT);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
}


void getDistance() {
  long duration;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2); 
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  delay(sr04Delay);
}

void loop() {

  if(runAuto) {
    getDistance();
    if(distance < MINDIST && twist_direction == FORWARD) {
      twist_direction = REVERSE; 
    }
    else if(distance > MAXDIST && twist_direction == REVERSE) {
      twist_direction = FORWARD; 
    }
  }

  if(Serial.available() > 0) {
    int in = Serial.parseInt();
    switch(in) {
      case 4:  
        on = false;
        digitalWrite(a1, LOW);
        digitalWrite(a2, LOW);
        digitalWrite(b1, LOW);
        digitalWrite(b2, LOW);
        break;
      case 1:
        on = true;
        break;
      case 2:
        twist_direction = FORWARD;
        break;
      case 3:
        twist_direction = REVERSE;
        break;
      }
  }


  if(on) {
    switch(twist_direction) {
      case FORWARD:
        digitalWrite(b1, LOW);
        digitalWrite(a2, LOW);
        digitalWrite(b2, HIGH);
        digitalWrite(a1, HIGH);
        break;
  
      case REVERSE:  
        digitalWrite(b2, LOW);
        digitalWrite(a1, LOW);
        digitalWrite(b1, HIGH);
        digitalWrite(a2, HIGH);
        break;            
      }
    }
}