FinalCodeVersion.ino

#define servoPin 9
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <EEPROM.h>
Adafruit_BNO055 bno = Adafruit_BNO055(55);


int motorLVOLT = 5;
int motorLGND = 6;
int IRSensor = 9;
int motorRVOLT = 11;
int turnC = 3;
int motorRGND = 10;
int prevTime = 0;
int s = 25;
int f = 38;
int t = 43; // 50
int cons = 100;

//   1
// 4   2
//   3
int servo0 = 3.5;
int i = 1;
int leftMSpeed = 200; //190
int rightMSpeed = 200; //200
int ang = 3;

volatile long motorPositionA = 0;
volatile long motorPositionB = 0;
//boolean stopAUTO = true;
void setup() {
    pinMode(motorLVOLT, OUTPUT);
    pinMode(motorRVOLT, OUTPUT);
    pinMode(motorLGND, OUTPUT);
    pinMode(motorRVOLT, OUTPUT);
    pinMode(IRSensor, INPUT);

    Serial.begin(9600);
    attachInterrupt(digitalPinToInterrupt(2), updateMotorPositionA, RISING);
    attachInterrupt(digitalPinToInterrupt(3), updateMotorPositionB, RISING);
     Serial.begin(9600);
  Serial.println("Orientation Sensor Test"); Serial.println("");
  
  /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }
  
  delay(1000);
    
  bno.setExtCrystalUse(true);
}


void forwardAndRight(){
  forward(50,1);
  align(2);
  forward(8.5,2);
  align(2);
}

void forwardAndLeft(){
  forward(50,1);
  align(4);
  forward(8.5,4);
  align(4);
}

void forward(int deljay, int head){
  // deljay-=2;
  deljay *= 20;
  analogWrite(motorLVOLT, leftMSpeed);
  analogWrite(motorRVOLT, rightMSpeed);
  analogWrite(motorLGND, 0);
  // delay(500);
  analogWrite(motorRGND, 0);
  delay(500);
  // delay((deljay*1150/50));
  align(head);
  // stop(100);
  // delay((deljay*1150/50));
  motorPositionB = 0;
  while (true){
    Serial.println(motorPositionB);
    delay(0.1);
    align(head);
    // changeSpeed(head);
    int sensorStatus = digitalRead(IRSensor);
    if (sensorStatus == 1){
      break;
    }
    if(motorPositionB < -1*deljay*42.5/49){
      break;
    }
    else{
        analogWrite(motorLVOLT, leftMSpeed);
        analogWrite(motorRVOLT, rightMSpeed);
        analogWrite(motorLGND, 0);
        analogWrite(motorRGND, 0);
    }
  }
  align(head);
  stop(100);
  motorPositionB=0;
}

void backward(int deljay, int head){
  // deljay-=2;
  deljay *= 20;
  analogWrite(motorLVOLT, 0);
  analogWrite(motorRVOLT, 0);
  analogWrite(motorLGND, leftMSpeed);
  // delay(500);
  analogWrite(motorRGND, rightMSpeed);
  delay(500);
  // delay((deljay*1150/50));
  align(head);
  // stop(100);
  // delay((deljay*1150/50));
  motorPositionB = 0;
  while (true){
    Serial.println(motorPositionB);
    delay(0.1);
    align(head);
    // changeSpeed(head);
    int sensorStatus = digitalRead(IRSensor);
    if (sensorStatus == 1){
      break;
    }
    if(motorPositionB > 1*deljay*42.5/49){
      break;
    }
    else{
      analogWrite(motorLVOLT, 0);
      analogWrite(motorRVOLT, 0);
      analogWrite(motorLGND, leftMSpeed);
      // delay(500);
      analogWrite(motorRGND, rightMSpeed);
    }
  }
  align(head);
  stop(100);
  motorPositionB=0;
}

void right(int deljay, int stoppo){
  digitalWrite(motorRVOLT, 200);
  digitalWrite(motorLVOLT, 0);
  digitalWrite(motorRGND, 0);
  digitalWrite(motorLGND, 200);
  delay(deljay);
  stop(stoppo);
}

void left(int deljay, int stoppo){
  digitalWrite(motorLVOLT, 200);
  digitalWrite(motorRVOLT, 0);
  digitalWrite(motorLGND, 0);
  digitalWrite(motorRGND, 200);
  delay(deljay);
  stop(stoppo);
}

void stop(int deljay){
   analogWrite(motorLGND, 0);
  analogWrite(motorRGND, 0);
  analogWrite(motorLVOLT, 0);
  analogWrite(motorRVOLT, 0);
  delay(deljay);
}

void align(int heading){
    if(heading == 1){
      sensors_event_t event; 
      bno.getEvent(&event);
          prevTime = 1;
      // Serial.print("X: ");
      // Serial.println(event.orientation.x, 4);
    while(event.orientation.x!=0){
      sensors_event_t event; 
      bno.getEvent(&event);
      if((event.orientation.x > 0 && event.orientation.x < ang) || (event.orientation.x > 360-ang && event.orientation.x < 360)||prevTime>150){
        break;
      }
      Serial.print("X: ");
      Serial.println(event.orientation.x, 4);
      if (event.orientation.x < 360 && event.orientation.x >= 180){
        right(turnC*(360-event.orientation.x), 100);
      }
      if (event.orientation.x > 0 && event.orientation.x < 180){
        left(turnC*event.orientation.x,100);
      }
    }
  }
  else if(heading == 2){
      sensors_event_t event; 
      bno.getEvent(&event);
      // Serial.print("X: ");
      // Serial.println(event.orientation.x, 4);
    prevTime = 1;
    while(event.orientation.x!=90){
      sensors_event_t event; 
      bno.getEvent(&event);
      if (event.orientation.x < 90+ang && event.orientation.x > 90-ang||prevTime>150){
        break;
      }
      Serial.println(prevTime++);
      // Serial.print("X: ");
      // Serial.println(event.orientation.x, 4);
      if ((event.orientation.x < 90 && event.orientation.x > 0)){
        right(turnC*(90-event.orientation.x), 100);
      }
      else if ((event.orientation.x < 360 && event.orientation.x > 270)){
        right(turnC*(450-event.orientation.x),100);
      }

      else if (event.orientation.x > 90 && event.orientation.x < 180){
        left(turnC*(event.orientation.x-90),100);
      }
      else if (event.orientation.x > 180 && event.orientation.x < 270){
        left(turnC*(event.orientation.x-90),100);
      }
    }
  }
    else if(heading == 4){
      sensors_event_t event; 
      bno.getEvent(&event);

      prevTime = 1;
      // Serial.print("X: ");
      // Serial.println(event.orientation.x, 4);
    while(event.orientation.x!=270){
      sensors_event_t event; 
      bno.getEvent(&event);
      if (event.orientation.x < 270+ang && event.orientation.x > 270-ang||prevTime>150){
        rightMSpeed = 200;
        break;
      }
      Serial.print("X: ");
      Serial.println(event.orientation.x, 4);
      if ((event.orientation.x < 360 && event.orientation.x > 270)){
        rightMSpeed = 235;
        left(turnC*(event.orientation.x-270), 100);
      }
      else if ((event.orientation.x < 90 && event.orientation.x > 0)){
        rightMSpeed = 200;
        left(turnC*25,100);
      }

      else if (event.orientation.x > 90 && event.orientation.x < 270){
        right(turnC*(270 - event.orientation.x),100);
      }
    }
  }
      else if(heading == 3){
                sensors_event_t event; 
          bno.getEvent(&event);
              prevTime = 1;
          // Serial.print("X: ");
          // Serial.println(event.orientation.x, 4);
        while(event.orientation.x!=180){
          sensors_event_t event; 
          bno.getEvent(&event);
          if((event.orientation.x > 180-ang && event.orientation.x < 180) || (event.orientation.x > 180 && event.orientation.x < 180+ang)||prevTime>150){
            break;
          }
          Serial.print("X: ");
          Serial.println(event.orientation.x, 4);
          if (event.orientation.x < 360 && event.orientation.x >= 180){
            left(turnC*(event.orientation.x-180), 100);
          }
          if (event.orientation.x > 0 && event.orientation.x < 180){
            right(turnC*(180-event.orientation.x),100);
          }
        }
      }
}


void updateMotorPositionA(){
  if(digitalRead(4) != digitalRead(2)){
    motorPositionA++;
  }
  else{
    motorPositionA--;
  }
}

void updateMotorPositionB(){
  if(digitalRead(3) != digitalRead(7)){
    motorPositionB++;
  }
  else{
    motorPositionB--;
  }
}

void alignfor(int head){
  align(head);
  forward(2,head);
}

void loop() {
  // About 2.4 sec per forward
  // About 1 sec per turn

  // *IMPORTANT REMINDERS*
  // Code should always start with delay(1000) and forward(s,1)
  // Then every consecutive straight including the one immediately after forward(t,1) should be forward(f, head)
  // Immediately after EVERY align the forward should be forward(t, head) and then move to the consecutives
  // Include a delay of cons in between every single command except the first two
  // Try NOT to go left very much

  // RT Code
  delay(1000);
  forward(s,1);
  delay(cons);
  align(2);
  delay(cons);
  forward(t,2);
  delay(cons);
  align(1);
  delay(cons);
  forward(t,1);
  delay(cons);
  align(4);
  delay(cons);
  forward(t,4);
  delay(cons);
  forward(f,4);
  delay(cons);
  align(3);
  delay(cons);
  forward(t,3);
  delay(cons);
  align(4);
  delay(cons);
  forward(t,4);
  delay(cons);
  align(1);
  delay(cons);
  forward(t,1);
  delay(cons);
  forward(f,1);
  delay(cons);
  forward(f,1);
  delay(cons);
  align(2);
  delay(cons);
  forward(t,2);
  delay(cons);
  forward(f,2);
  delay(cons);
  align(3);
  delay(cons);
  forward(t,3);
  delay(cons);
  forward(f,3);
  delay(cons);
  align(2);
  delay(cons);
  forward(t,2);
  delay(cons);
  align(3);
  delay(cons);
  forward(t,3);
  delay(cons);
  align(4);
  delay(cons);
  forward(t,4);
  delay(cons);
  forward(f,4);
  delay(cons);
  forward(f,4);
  delay(cons);
  align(1);
  delay(cons);
  forward(s,1);
  delay(cons);

while(1==1){
    stop(100);
  }
}
// lolo