KartBot rev3

// Una's crazed Go-Kart Project, aka, KartBot
// Dual OSMC's controlling a wheelchair base
// Creative Commons License. Written by Brian Gosney 2012
// Revision 3, pretty much.

//#include <LiquidCrystalFast.h>

//LiquidCrystalFast lcd(7,8,9,10,11,12,13);


int readings[100];
byte readingNumber=0;
long averageMe = 0;
int brakeRaw;
int tpsRaw;
byte dutyCycle;
byte brakeLevel;
byte tpsLevel;
boolean motorEnabled= false;
boolean braking = false;
boolean goForward = true;

#define disablePin 5
#define leftForward 14
#define leftBackward 24
#define rightForward 15
#define rightBackward 25
#define brakeControl 16
#define throttleSensor 38
#define brakeSensor 39
#define steeringSensor 40 // Not yet implemented - placeholder for now
#define leftMotorTemp 41 // Moving to I2C temp sensors asap.
#define rightMotorTemp 42




void setup() {
//  Serial.begin(9600);
  pinMode(disablePin, OUTPUT);
  digitalWrite(disablePin, HIGH); // Go high immediately to prevent twitching on startup. High = disabled.
  pinMode(leftForward, OUTPUT);
  pinMode(leftBackward, OUTPUT);
  pinMode(rightForward, OUTPUT);
  pinMode(rightBackward, OUTPUT);
  pinMode(brakeControl, OUTPUT);
  digitalWrite(brakeControl, HIGH); // Release the brake, PWM mode so we can ease it back on.
  /*  lcd.begin(20,4);
   lcd.setCursor(0,0);
   lcd.print(F("Throttle:    %"));
   lcd.setCursor(0,1);
   lcd.print(F("Brake:    %")); */
  pinMode(throttleSensor, INPUT);
  pinMode(brakeSensor, INPUT);
}

void applyBrakes() {
  if (brakeRaw < 800) {
    digitalWrite(disablePin, LOW); // engage motor electric brakes
    digitalWrite(brakeControl, HIGH); // make sure mag brakes are released
  }
  else {
    digitalWrite(brakeControl, LOW); // lock up the mag brakes!
  }
  braking = true;
}

void stopMotors() {
  // stop the motors
  motorEnabled = false;
  digitalWrite(disablePin, HIGH);
  analogWrite(leftForward, 0);
  analogWrite(rightForward, 0);
}

void readInputs() {
  tpsRaw = analogRead(throttleSensor);
  brakeRaw = analogRead(brakeSensor);
}

void driveMotors() {
  dutyCycle = map(tpsRaw, 0, 1024, 0, 255);
  // Serial.print("Motor Duty Cycle ");
  // Serial.println(dutyCycle);
  if (goForward) {
    analogWrite(leftForward, dutyCycle);
    analogWrite(rightForward, dutyCycle);
  }
  else {
    analogWrite(leftBackward, dutyCycle);
    analogWrite(rightBackward, dutyCycle);
  }
  if (!motorEnabled) {
    digitalWrite(disablePin, LOW); // enable the motor controllers
    motorEnabled = true;
  }

}



void loop() {
  readInputs();
  //  lcd.setCursor(10,0);
  //  tpsLevel = map(tpsRaw, 0, 1023, 0, 100);
  //  brakeLevel = map(brakeRaw, 0, 1023, 0, 100);
  /*  lcd.print(tpsLevel);
   lcd.print("%   ");
   lcd.setCursor(7,1);
   lcd.print(brakeLevel);
   lcd.print("%   "); */
  //  Serial.print("TPS: ");
  //  Serial.println(tpsLevel);
  if (tpsRaw > 3) { // Gas pedal is pressed
    if (brakeRaw < 10) { // Brake pedal isn't
      if (braking) { // Brake pedal had been pressed, but isn't now
        digitalWrite(brakeControl, HIGH); // Release magnetic brakes
        digitalWrite(disablePin, HIGH); // Release motor electric brakes
        braking = false;
      }
      // drive the motors
      driveMotors();
    }
  }
  else if (motorEnabled) {
    stopMotors();
  }
  if ((!motorEnabled) and (brakeLevel > 5)) { // let's use the brakes!
    applyBrakes();
  }

}