Race CarΒΆ

The map changed, so I changed my code slightly:

const int MOTOR_LEFT_POS = 7;
const int MOTOR_LEFT_NEG = 8;
const int MOTOR_RIGHT_POS = 12;
const int MOTOR_RIGHT_NEG = 13;
const int MOTOR_LEFT_PWM = 10;
const int MOTOR_RIGHT_PWM = 11;

#include <Wire.h>
#define uchar unsigned char
uchar t;
//void send_data(short a1,short b1,short c1,short d1,short e1,short f1);
uchar data[16];
//#define DEBUG
#define threshold 110
void setup() {
pinMode(MOTOR_LEFT_POS, OUTPUT);
pinMode(MOTOR_LEFT_NEG, OUTPUT);
pinMode(MOTOR_RIGHT_POS, OUTPUT);
pinMode(MOTOR_RIGHT_NEG, OUTPUT);
digitalWrite(MOTOR_LEFT_POS, HIGH);
digitalWrite(MOTOR_RIGHT_POS, LOW);
digitalWrite(MOTOR_LEFT_NEG, LOW);
digitalWrite(MOTOR_RIGHT_NEG, HIGH);
Serial.begin(9600);
Serial.println("Serial Connected");
Wire.begin();        // join i2c bus (address optional for master)
t = 0;
}

void drive(int left, int right) {
if (left < 0) {
digitalWrite(MOTOR_LEFT_POS, LOW);
digitalWrite(MOTOR_LEFT_NEG, HIGH);
} else {
digitalWrite(MOTOR_LEFT_POS, HIGH);
digitalWrite(MOTOR_LEFT_NEG, LOW);
}

if (right < 0) {
digitalWrite(MOTOR_RIGHT_POS, HIGH);
digitalWrite(MOTOR_RIGHT_NEG, LOW);
} else {
digitalWrite(MOTOR_RIGHT_POS, LOW);
digitalWrite(MOTOR_RIGHT_NEG, HIGH);
}

analogWrite(MOTOR_LEFT_PWM, abs(left));
analogWrite(MOTOR_RIGHT_PWM, abs(right));
}


char dir='N';

void loop() {
Wire.requestFrom(9, 16);    // request 16 bytes from slave device #9
while (Wire.available())   // slave may send less than requested
{
data[t] = Wire.read(); // receive a byte as character
if (t < 15)
  t++;
else
  t = 0;
}


int readingLeft = (data[0] + data[2] + data[4]) / 3;
int readingRight = (data[10] + data[12] + data[14]) / 3;
int readingMid = (data[6] + data[8]) / 2;
#ifdef DEBUG
for (int i=0;i<8;i++){
Serial.print(data[i*2]);
Serial.print(' ');
}
Serial.println();
#endif
if (dir=='N'){
bool flag=false;
for (int i=0;i<8;i++){
if (data[i*2]<=threshold)
  flag=true;
}
if (!flag){
drive(255,255);
return;
}
if (data[0]<threshold){
    if (data[2]<threshold){
      drive(255,-100);
    } else {
      drive(255,255);
    }
} else if (data[14]<threshold){
    if (data[12]<threshold){
      drive(255,255);
    } else {
      drive(255,-100);
    }
}
}


}