KRISYS Robot

/*
   LP_Krisys_V3
   Author:  Austin Carter

   Version: 3 4/19/18
   Funtional Code with one User defined function  

   Version 3.1 6/8/18
   Removing trivial components
*/
int motor_speed[] = {0, 0, 0};
int sensor_data[] = {0, 0, 0};
int L = 0;
int lost_id = 0;
int R = 0;
int s;

// User defined function
void state_machine(int state);

void setup() {
  // put your setup code here, to run once
  // Begin Serial Monitor for Debugging
  Serial.begin(9600);
  Serial.println("##################################################");
  Serial.println("##########    _______'s Krisys Robot    ##########");
  Serial.println("##################################################");
  Serial.print("Please wait");

  // Motor control pins
  pinMode(RED_LED, OUTPUT); // routed to IN1 on PDB ; silkscreen P64 
  pinMode(30, OUTPUT); // routed to IN2 on PDB ; silkscreen P50
  pinMode(31, OUTPUT); // routed to IN3 on PDB ; silkscreen P17
  pinMode(11, OUTPUT); // routed to IN4 on PDB ; silkscreen P15

  // Sensor Board information pins
  // Sensor info is digital active low
  pinMode(5, INPUT); // O1 on sensor board ; silkscreen P61 ; Sensor 1 info
  pinMode(6, INPUT); // O2 on sensor board ; silkscreen P59 ; Sensor 2 info
  pinMode(7, INPUT); // O3 on sensor board ; silkscreen P05 ; Sensor 3 info
  pinMode(8, OUTPUT); // LED on sensor board ; silkscreen P62 ; Front LED control

  // Tell motor control to begin stopped
  analogWrite(RED_LED, 0);
  digitalWrite(30, LOW);
  analogWrite(31, 0);
  digitalWrite(11, LOW);

  // Tell sensor board LED pin what to start at
  // Control of LEDs can be modified during run of code using 'digitalWrite(8, ____)'
  // Blink LEDs on sensor board to allow time for students to move away from robot at start of race
  digitalWrite(8, HIGH);
  delay(1500);
  Serial.print(".");
  digitalWrite(8, LOW);
  delay(1500);
  digitalWrite(8, HIGH);
}

void loop() {
  // put your main code here, to run repeatedly
  // Read sensor data and store to data array
  sensor_data[0] = digitalRead(5);
  sensor_data[1] = digitalRead(6);
  sensor_data[2] = digitalRead(7);

  // Print sensor data
  Serial.print("Sensor Values: ");
  Serial.print(sensor_data[2]);
  Serial.print(sensor_data[1]);
  Serial.println(sensor_data[0]);

  // Convert state from binary code to decimal
  s = 4 * sensor_data[2] + 2 * sensor_data[1] + sensor_data[0];
  Serial.print("State: ");
  Serial.println(s);
  Serial.print("Location: ");
  
  // Pass state to state machine user defined function
  // State machine will determine motor speed and direction
  state_machine(s); // USER DEFINED FUNCTION: code written for function later on
  
  L = motor_speed[0];
  R = motor_speed[1];

// Move motors
if (L >= 0) {
    // Speed goes from 0 min to 255 max
    analogWrite(31, L);
    digitalWrite(11, LOW);
  }
  else if (L < 0) {
    // reverse direction max speed occurs at -1 and stop occurs at -255
    // makes value postive since analogWrite only accepts positive values
    analogWrite(31, L + 255);
    digitalWrite(11, HIGH);
  }
  if (R >= 0) {
    // Speed goes from 0 min to 255 max
    analogWrite(RED_LED, R);
    digitalWrite(30, LOW);
  }
  else if (R < 0) {
    // Reverse direction max speed occurs at -1 and stop occurs at -255
    // Makes value postive since analogWrite only accepts positive
    analogWrite(RED_LED, R + 255);
    digitalWrite(30, HIGH);
  }
delay(1); // delay in milliseconds ; allow time for motor PWM to have effect
// end of main loop
}

void state_machine(int state) {
  // variables that will only exist in the function
  int L_spd = 0;
  int R_spd = 0;
  
  // change motor speed based on location
  // locations determined as if user is driving the robot
  switch (state) {
    case 1:
      // when sesors read 001
      Serial.println("SR");
      L_spd = 175;
      R_spd = 200;
      lost_id = 1;
      break;

    case 3:
      // when sensors read 011
      Serial.println("R");
      L_spd = 150;
      R_spd = 200;
      lost_id = 1;
      break;

    case 4:
      // when sensors read 100
      Serial.println("SL");
      L_spd = 200;
      R_spd = 175;
      lost_id = 0;
      break;

    case 5:
      // when sensors read 101
      Serial.println("C");
      L_spd = 200;
      R_spd = 200;
      break;

    case 6:
      // when sensors read 110
      Serial.println("L");
      L_spd = 200;
      R_spd = 150;
      lost_id = 0;
      break;

    case 7:
      // when sensor read 111 ; multi state
      Serial.println("Multi");
      if (lost_id == 0) {
        // if lost from left
        L_spd = 100;
        R_spd = -100;
        lost_id = 0;
      }
      else {
        // if lost from right
        L_spd = -100;
        R_spd = 100;
        lost_id = 1;
      }
      break;

    default:
      // if location detection is error state or unexpected ; go slow reverse
      L_spd = -128;
      R_spd = -128;
      break;
  }
  // Stores speed and lost values to global array
  motor_speed[0] = L_spd;
  motor_speed[1] = R_spd;
} // end of user defined function