CG_MarioKart/CG_MarioKart.ino

/*
 RC PulseIn Joystick Servo Control
 By: Nick Poole
 SparkFun Electronics
 Date: 5
 License: CC-BY SA 3.0 - Creative commons share-alike 3.0
 use this code however you'd like, just keep this license and
 attribute. Let me know if you make hugely, awesome, great changes.
 */

const int ch1_pin = 10 ;
const int ch2_pin = 11 ;

int ch1; // Here's where we'll keep our channel values
int ch2;

int move; // Forward/Back speed
int turn; // Turning Factor
int speeds_pin[2] = {5, 6};
int dirs_pin[2] = {4, 7};
//int pwm_a = 5;  //PWM control for motor outputs
//int pwm_b = 6;  //PWM control for motor outputs
//int dir_a = 4;  //direction control for motor outputs
//int dir_b = 7;  //direction control for motor outputs

int threshold = 30 ;

int manualMaxSpeed = 1024 ;
const int maxSpeed_pin = A0;
const int driftCorrection_pin = A1 ;
int maxSpeed = 1024;
float driftCorrection = 511 ;


int middleThreshold (int RCvalue, int threshold) {  // 

  if ( (RCvalue < 1500 + threshold) && (RCvalue > 1500 - threshold) ) { RCvalue = 1500;}
  return RCvalue ;
  
}

void setup() {

pinMode(ch1_pin, INPUT); // Set our input pins as such
pinMode(ch2_pin, INPUT);

Serial.begin(9600); // Pour a bowl of Serial (for debugging)

  for(int i; i<2; i++){
    pinMode(speeds_pin[i], OUTPUT);
    pinMode(dirs_pin[i], OUTPUT);
    analogWrite(speeds_pin[i], 0) ;
  }
//  pinMode(pwm_a, OUTPUT);  //Set control pins to be outputs
//  pinMode(pwm_b, OUTPUT);
//  pinMode(dir_a, OUTPUT);
//  pinMode(dir_b, OUTPUT);

//  analogWrite(pwm_a, 0);  
//  analogWrite(pwm_b, 0);
}


void loop() {

//  Read and scale analog inputs
//  maxSpeed = map (analogRead(maxSpeed_pin), 0, 1024, 0, 255);
  maxSpeed = manualMaxSpeed ;
//  driftCorrection = analogRead(driftCorrection_pin) ;
  driftCorrection = 511 ;
  driftCorrection = driftCorrection / 512 - 1 ; // -1..1
  driftCorrection *= 0.5 ;

  Serial.print("maxSpeed:"); //Serial debugging stuff
  Serial.println(maxSpeed);
  Serial.print("correction:"); //Serial debugging stuff
  Serial.println(driftCorrection);
  

  //Read RC values and give wide center point
  ch1 = pulseIn(ch1_pin, HIGH, 25000); // Read the pulse width of  
  ch2 = pulseIn(ch2_pin, HIGH, 25000); // each channel
  ch1 = middleThreshold(ch1, threshold);
  ch2 = middleThreshold(ch2, threshold);
  if(ch1 == 0) {ch1 = 1500;}
  if(ch2 == 0) {ch2 = 1500;}
  
  Serial.print("ch1:"); //Serial debugging stuff
  Serial.println(ch1);
  
  Serial.print("ch2:"); //Serial debugging stuff
  Serial.println(ch2);
  
  //Refresh and scale raw move values
  move = map(ch2, 1000,2000, -255, 255); //center over zero
  move = constrain(move, -255, 255);
  turn = map(ch1,1000,2000,-255,255);
 

  Serial.print("move:"); //Serial debugging stuff
  Serial.println(move);
  
  Serial.print("turn:"); //Serial debugging stuff
  Serial.println(turn);
  
  //calculate each motor speed
  int speeds[2] = {0, 0} ;
  
  int Dir = move>0 ? 1 : 0;
//  move = min( abs(move), maxSpeed) ;
  move = abs(move);
  turn = constrain(turn, -move, move);
  
  if (turn>0) { speeds[0] = move; speeds[1] = move - turn  ; }
  if (turn<=0) { speeds[0] = move + turn ; speeds[1] = move   ;  }

//  if (driftCorrection>0) { speeds[0] *= 1-driftCorrection ; }
//  if (driftCorrection<=0) { speeds[1] *= 1+driftCorrection ; }
//  


  Serial.print("speeds: "); //Serial debugging stuff
  Serial.print(speeds[0]);
  Serial.print(" - "); 
  Serial.println(speeds[1]);
  
  //apply to driver
  for(int i =0; i<2; i++) {
    speeds[i] = abs( speeds[i]);
    
    digitalWrite(dirs_pin[i], Dir) ;
    analogWrite(speeds_pin[i], speeds[i]) ;
    
  }

  Serial.println();

}