/* 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 ; const int maxSpeed_pin = A0; const int driftCorrection_pin = A1 ; int maxSpeed = 0; float driftCorrection = 0 ; 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); driftCorrection = analogRead(driftCorrection_pin) ; driftCorrection -= 512; driftCorrection /= 8 ; 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); //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) ; 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] -= driftCorrection*; speeds[1] = move - turn ; } // if (driftCorrection<=0) { speeds[0] = move + turn ; speeds[1] = move ; } // 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(); }