challenge.ino

#include <Adafruit_NeoPixel.h>

const int motors[2][4] = {{10, 11, 3, 1}, {12, 13, 9, 2}};
const int rightChannel = 1;
const int leftChannel = 2;
const int analogCenter = 1000;
const int analogRange = 400;
const int zeroRange = 40;
const int chan = 6;
const int motorMax = 255;
int value[chan];

Adafruit_NeoPixel strip = Adafruit_NeoPixel(16, 6, NEO_GRB + NEO_KHZ800);

const int rangeThreshold = 40;

unsigned long start;
int count;

void setup() {
  for (int i=0;i<2;++i) for (int j=0;j<3;++j) pinMode(motors[i][j], OUTPUT);
  
  for (int i=0;i<2;++i) digitalWrite(motors[i][2], HIGH);
  pinMode(A0, INPUT);
  digitalWrite(A0, HIGH);
  
  pinMode(A4, INPUT);
  
  Serial.begin(56700);
  strip.begin();
  for (int i=0;i<16;++i) {
    strip.setPixelColor(i, 0, 255, 0);
  }
  strip.show();
  start = millis();
  turn(200);
  delay(800);
}

void loop() {
  if (millis() - start < 30000) {
    int r = analogRead(A4);
    Serial.println(r, DEC);
    if (r > rangeThreshold) {
      count++;
      if (count > 5) {
        mv(200);
        delay(500);
      }
    } else {
      count = 0;
      turn(75);
    }
  } else {
    while (pulseIn(A0, LOW) < 5000) {}
    getChannels();
    
    for (int i=0;i<2;++i) {
      Serial.print(motorChannelValue(motors[i][3]), DEC);
      Serial.print(" ");
      setMotor(i, motorChannelValue(motors[i][3]));
    }
    Serial.println();
  }
  delay(15);
}

void getChannels() {
  for(int x=0; x < chan; ++x) {
    value[x] = pulseIn(A0, LOW);
  }
}

void turn(int spd) {
  setMotor(0, spd);
  setMotor(1, spd);
}

void mv(int spd) {
  setMotor(0, spd);
  setMotor(1, -spd);
}

void setMotor(int motor, int value) {
  motorDir(motor, value / abs(value));
  motorSpeed(motor, abs(value));
}

void motorDir(int motor, int dir) {
  if (dir == 0) {
    digitalWrite(motors[motor][0], LOW);
    digitalWrite(motors[motor][0], LOW);
  } else {
    int md = binToSign(motor);
    dir *= md;
    digitalWrite(motors[motor][0], signToBin(-dir));
    digitalWrite(motors[motor][1], signToBin(dir));
  }
}

void motorSpeed(int motor, int spd) {
  analogWrite(motors[motor][2], spd);
}

int binToSign(int value) {
  return value * 2 - 1;
}

int signToBin(int value) {
  return (value + 1) / 2;
}

int radioValue(int chan) {
  int radio = value[chan] - analogCenter;
  if (radio < -analogRange) radio = -analogRange;
  if (radio > analogRange) radio = analogRange;
  if (abs(radio) < zeroRange) radio = 0;
  return radio;
}

int motorValue(int radio) {
  return (int) ((long) radio * motorMax / analogRange);
}

int motorChannelValue(int chan) {
  return motorValue(radioValue(chan));
}