|
|
@@ -0,0 +1,99 @@
|
|
|
+/*
|
|
|
+ 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 ch3;
|
|
|
+
|
|
|
+int move; // Forward/Back speed
|
|
|
+int turn; // Turning Factor
|
|
|
+
|
|
|
+int pwm_a = 4; //PWM control for motor outputs
|
|
|
+int pwm_b = 7; //PWM control for motor outputs
|
|
|
+int dir_a = 5; //direction control for motor outputs
|
|
|
+int dir_b = 6; //direction control for motor outputs
|
|
|
+
|
|
|
+int threshold = 30 ;
|
|
|
+
|
|
|
+
|
|
|
+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)
|
|
|
+
|
|
|
+ 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() {
|
|
|
+
|
|
|
+ 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);
|
|
|
+
|
|
|
+
|
|
|
+ move = map(ch2, 1000,2000, -255, 255); //center over zero
|
|
|
+ move = constrain(move, -255, 255); //only pass values whose absolutes are
|
|
|
+ //valid pwm values
|
|
|
+
|
|
|
+ /*What we're doing here is determining whether we want to move
|
|
|
+ forward or backward*/
|
|
|
+ if(move>0){digitalWrite(dir_a, 1);digitalWrite(dir_b, 1);};
|
|
|
+ if(move<0){digitalWrite(dir_a, 0);digitalWrite(dir_b, 0); move=abs(move);};
|
|
|
+
|
|
|
+ /*Here we're determining whether a left or a right turn is being
|
|
|
+ executed*/
|
|
|
+ turn = map(ch1,1000,2000,-255,255);
|
|
|
+ turn = constrain(turn, -255, 255);
|
|
|
+
|
|
|
+ /*This is where we do some mixing, by subtracting our "turn"
|
|
|
+ variable from the appropriate motor's speed we can execute
|
|
|
+ a turn in either direction*/
|
|
|
+ if(turn>=0){analogWrite(pwm_b, move-turn); analogWrite(pwm_a, move);};
|
|
|
+ if(turn<0){turn=abs(turn); analogWrite(pwm_a, move-turn); analogWrite(pwm_b, move);};
|
|
|
+
|
|
|
+
|
|
|
+ Serial.print("move:"); //Serial debugging stuff
|
|
|
+ Serial.println(move);
|
|
|
+
|
|
|
+ Serial.print("turn:"); //Serial debugging stuff
|
|
|
+ Serial.println(turn);
|
|
|
+
|
|
|
+ Serial.print("move-turn:"); //Serial debugging stuff
|
|
|
+ Serial.println(move-turn);
|
|
|
+
|
|
|
+ Serial.println(); //Serial debugging stuff
|
|
|
+ Serial.println();
|
|
|
+ Serial.println();
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
