/* Complete RC Control with.. Autonomous guidance with simple if statements and variable speeds */ #include // Pins for teensy const int lPhoto = A0; // Left Photoresistor pin const int rPhoto = A1; // Right Photoresistor pin const int pPinFR = A2; // front right prox pin const int pPinFL = A3; // front left prox pin const int pPinBR = A4; // back right prox pin const int pPinBL = A5; // back left prox pin const int pPinFFR = A6; // front forward right prox pin const int pPinFFL = A7; // front forward left prox pin const int rMotor = 1; // right motor controller pin const int lMotor = 2; // left motor controller pin const int clawServo = 4; // claw servo pin // Create Variables to hold the Receiver signals int Ch1, Ch2, Ch3, Ch4, Ch5, Ch6; int Rwheel; // Variable to hold R wheel speed int Lwheel; // variable to hold L wheel speed int Boom; // servo position for med kit device const int LED = 13; // Onboard LED location int lPhotoVal; // Variable to store L photoresistor value int rPhotoVal; // Variable to store R photoresistor value int proxFL; // Variable to store Sharp Sensor value Front Left int proxFR; // Variable to store Sharp Sensor value Front Right int proxBL; // Variable to store Sharp Sensor value Back Left int proxBR; // Variable to store Sharp Sensor value Back Right int proxFFL, proxFFR; // Variables for Front Forward Left/Right prox sensors int rSpeed, lSpeed; // Variables to hold autonomous speed changes for each wheel // Change values below for speed calibration const int LEFT_FOR = 1700; const int LEFT_REV = 1300; const int RIGHT_FOR = 1700; const int RIGHT_REV = 1300; // Create Servo Objects as defined in the Servo.h files Servo L_Servo; // Servo DC Motor Driver (Designed for RC cars) Servo R_Servo; // Servo DC Motor Driver (Designed for RC cars) Servo B_Servo; // Servo DC Motor Driver (Designed for RC cars) //************************************************************** //*********************** Setup ****************************** //************************************************************** void setup() { // Set the pins that the transmitter will be connected to all to input pinMode(12, INPUT); //I connected this to Chan1 of the Receiver pinMode(11, INPUT); //I connected this to Chan2 of the Receiver pinMode(10, INPUT); //I connected this to Chan3 of the Receiver pinMode(9, INPUT); //I connected this to Chan4 of the Receiver pinMode(8, INPUT); //I connected this to Chan5 of the Receiver pinMode(7, INPUT); //I connected this to Chan6 of the Receiver pinMode(LED, OUTPUT);//Onboard LED to output for diagnostics // Attach Speed controller that acts like a servo to the board R_Servo.attach(rMotor); //Pin 1 L_Servo.attach(lMotor); //Pin 2 B_Servo.attach(clawServo); //Pin 3 B_Servo.writeMicroseconds(1600); // open the claw servo //Flash the LED on and Off 4x before entering main loop for (int i = 0; i < 5; i++) { digitalWrite(13, HIGH); delay(200); digitalWrite(13, LOW); delay(200); } // set wheels to zero //R_Servo.writeMicroseconds(1500); //L_Servo.writeMicroseconds(1500); Serial.begin(9600); } //************************ loop() **************************** //********************** Main Loop *************************** //************************************************************** void loop() { //checkProxSensors(); //printProxSensors(); //checkLightSensors(); //printLightSensors(); // Make sure remote is on and receiver is connected if (Ch5Check() != -1) { // UNCOMMENT TO DEBUG LIGHT OR CHUTE MODE // if (Ch5Check()) autonomousChute(); // else if (Ch6Check()) autonomousLight(); // else DriveServosRC(); // Check for autonoumous mode if (Ch5Check() > 0) { //Serial.println("AUTONOMOUS MODE ON"); autonomous(); } else { //Serial.println("RC MODE ON"); DriveServosRC(); } } // // blink LED but don't move if remote is off // else // { // stopBot(1); // digitalWrite(13, HIGH); // delay(200); // digitalWrite(13, LOW); // delay(200); // } } //********************** Ch5Check() ************************** //********************** Test Channel 5 ********************** //************************************************************** // Channel 5 HI puts robot in autonomous Chute navigation mode // Returns TRUE if ch 5 is high, FALSE if ch 5 is low (pin 8) int Ch5Check() { Ch5 = pulseIn(8, HIGH, 21000); // Capture pulse width on Channel 5 if (Ch5 > 1600) { digitalWrite(LED, HIGH); return 1; } else { if (Ch5 > 500) { digitalWrite(LED, LOW); return 0; } // return -1 if teensy isn't receiving remote data (Ch5 = 0) else return -1; } } //********************** autonomous() ************************** //************** Autonomous Course Structure ***************** //************************************************************** void autonomous() { autoRampToChute(); autoWall(); autoMedkit(); // stop condition is >850-900 Serial.println("AUTONOMOUS MODE COMPLETE"); while (Ch5Check() > 0) { delay(10); } } //********************** autoRampToChute() ************************** // Navigate ramp and chute autonomously //********************************************************************* // Go up incline and down decline autonomously // Navigate to light, line up straight, then proceed forward // maybe check front prox sensors to avoid chute walls on decline void autoRampToChute() { // Thresholds to change int rampStopThresh = 200; // distance for stopping at beginning of ramp int rampAlignThresh = 100; // difference between proxFF sensors int frontThresh = 300; // distance from front prox sensors int backThresh = 500; // distance from back prox sensors int chuteStopThresh = 400; // distance from wall // Light navigation function iterates // Stops at given threshold Serial.println("NAVIGATING TO RAMP LIGHT..."); autoLight(rampStopThresh, 1); Serial.println("RAMP LIGHT NAVIGATED"); delay(2000); // Serial.println("ALIGNING WITH RAMP..."); // autoAlign(rampAlignThresh); // Serial.println("ALIGNED WITH RAMP"); // delay(3000); // Drive straight over ramp // Enter chute navigation // break when close to the wall Serial.println("RUNNING RAMP AND CHUTE"); while (Ch5Check() > 0) { checkProxSensors(); printProxSensors(); // wall is close to front of robot if (proxFFL > chuteStopThresh && proxFFR > chuteStopThresh) { Serial.println("STOP"); stopBot(1); break; } // Robot too close on front right or back left if ((proxFR >= frontThresh || proxBL >= backThresh)) { // TURN LEFT SLOWLY; enter right wheel value (forward >1500) and delay Serial.println("MOVE: Left"); TLeftSlow(RIGHT_FOR, 1); //frontThresh = frontThresh - 5; } // Robot too close on front left or back right else { if ((proxFL >= frontThresh || proxBR >= backThresh)) { // TURN RIGHT SLOWLY; enter left wheel value (forward >1500) and delay Serial.println("MOVE: Right"); TRightSlow(LEFT_FOR, 1); } // Proceed forward else { Serial.println("MOVE: Forward"); Forward(1700, 10); } } } } //********************** autoWall() ************************** // Climb wall autonomously //************************************************************** // 2 stages - climb first/second step / drop slowly, get back leg off fast void autoWall() { // move forward slow while (Ch5Check() > 0) Forward(1575, 20000); stopBot(2000); //move forward fast while (Ch5Check() > 0) Forward(2000, 5000); stopBot(2000); } //********************** autoMedkit() ************************** // Navigate to basket and drop medkit //************************************************************** void autoMedkit() { // Find light and stop at threshold int stopThreshold = 900; autoLight(stopThreshold, 0); delay(2000); // open servo hook B_Servo.writeMicroseconds(1300); } //*********************** autoLight() ************************ //********************* Light Navigation ********************* //************************************************************** // This is only for light navigation // Seeks a light then stops at a certain distance from it void autoLight(int stopThreshold, int flag) { // Threshold values to change const int detectThresh = 350; // Threshold for detecting the light const int diffThresh = 200; // Threshold for light value difference int valDiff; // Variable to store difference between photo values int rSpeed = RIGHT_FOR; int lSpeed = LEFT_FOR; valDiff = checkLightSensors(); //printLightSensors(); // Turn around until sensors see light Serial.println("LOOKING FOR LIGHT..."); while (Ch5Check() > 0 && lPhotoVal > detectThresh) { valDiff = checkLightSensors(); //printLightSensors(); TLeftSlow(1700, 1); } Serial.println("LIGHT DETECTED..."); stopBot(1); delay(2000); // Keep light centered between the two light sensors Serial.println("NAVIGATING TO LIGHT"); int COUNT = 0; while (Ch5Check() > 0) { Serial.println(COUNT); COUNT++; // break loop if robot is close checkProxSensors(); //printProxSensors(); if (flag == 0) { if (proxFFL > stopThreshold || proxFFR > stopThreshold) { stopBot(1); break; } } else { if (COUNT > 150) // CHANGE IF START IS FURTHER THAN 3 FEET { stopBot(1); break; } } valDiff = checkLightSensors(); printLightSensors(); // if difference is greater than threshold if (valDiff > diffThresh) { // left is brighter than right if (lPhotoVal < rPhotoVal) { Serial.println("TURN LEFT"); rSpeed = rSpeed + 5; if (rSpeed >= RIGHT_FOR + 100) rSpeed = RIGHT_FOR + 100; TLeftFast(rSpeed, 1); //Serial.println(rSpeed); } // right is darker than left else { Serial.println("TURN RIGHT"); lSpeed = lSpeed + 5; if (lSpeed >= LEFT_FOR + 100) lSpeed = LEFT_FOR + 100; TRightFast(lSpeed, 1); //Serial.println(lSpeed); } } else { rSpeed = RIGHT_FOR; lSpeed = LEFT_FOR; Serial.println("FORWARD"); Forward(RIGHT_FOR, 10); } //printSensors(); } } //*********************** autoAlign() ************************ //******************* Robot/Object Aligning ****************** //************************************************************** void autoAlign(const int alignThreshold) { checkProxSensors(); int proxFF_Diff = proxFFL - proxFFR; // Difference in proxFF values // iterate through lining up robot while (Ch5Check() > 0 && abs(proxFF_Diff) > alignThreshold) { checkProxSensors(); proxFF_Diff = proxFFL - proxFFR; // right side is closer: left wheel forward if (proxFF_Diff < 0) TLeftSlow(1600, 1); // left side is closer: right wheel forward else TRightSlow(1600, 1); } stopBot(1); } //********************** CheckProxSensors() ************************ //****************** Checks all proximity sensors ****************** //******************************************************************** // read all 6 proximity sensors and update values void checkProxSensors() { // Update variables with sensor readings proxFR = analogRead(pPinFR); proxFL = analogRead(pPinFL); proxBR = analogRead(pPinBR); proxBL = analogRead(pPinBL); proxFFR = analogRead(pPinFFR); proxFFL = analogRead(pPinFFL); } //******************** checkLightSensors() ************************** // Check value of Sensors Stop bot if object is close //******************************************************************* // checking light sensors int checkLightSensors() { rPhotoVal = analogRead(rPhoto); lPhotoVal = analogRead(lPhoto); return abs(rPhotoVal - lPhotoVal); // looking for threshold } //************************************************************************ // Autonomous drive functions //************************************************************************ //***************** Forward(int Dlay) *********************** // Move the robot Slowly Forward //************************************************************** void Forward(int wheelSpeed, int Dlay) { int rValMap = map(wheelSpeed, 1000, 2000, 2000, 1000); R_Servo.writeMicroseconds(rValMap); // sets the servo position L_Servo.writeMicroseconds(wheelSpeed); // sets the servo position delay(Dlay); } //***************** Reverse(int Dlay) *********************** // Reverse the robot //************************************************************** void Reverse(int Dlay) { int rValMap = map(RIGHT_REV, 1000, 2000, 2000, 1000); R_Servo.writeMicroseconds(rValMap); // sets the servo position L_Servo.writeMicroseconds(LEFT_REV); // sets the servo position delay(Dlay); } //***************** stopBot(int Dlay) *********************** // Stop the robot //************************************************************** void stopBot(int Dlay) { R_Servo.writeMicroseconds(1500); // sets the servo position L_Servo.writeMicroseconds(1500); // sets the servo position delay(Dlay); } //************* TLeftSlow(int rVal,int Dlay) ******************* // left wheel slow pivot //************************************************************** void TLeftSlow(int rVal, int Dlay) { // map the right value so the numbers are consistent with the left servo int rValMap = map(rVal, 1000, 2000, 2000, 1000); R_Servo.writeMicroseconds(rValMap); // sets the servo position L_Servo.writeMicroseconds(1500); // sets the servo position delay(Dlay); } //************* TRightSlow(int lVal,int Dlay) ****************** // right wheel slow pivot //************************************************************** void TRightSlow(int lVal, int Dlay) { R_Servo.writeMicroseconds(1500); // sets the servo position L_Servo.writeMicroseconds(lVal); // sets the servo position delay(Dlay); } //************* TLeftFast(int rVal,int Dlay) ******************* // moving left turn //************************************************************** void TLeftFast(int rVal, int Dlay) { // map the right value so the numbers are consitent with the left servo int rValMap = map(rVal, 1000, 2000, 2000, 1000); R_Servo.writeMicroseconds(rValMap); // sets the servo position L_Servo.writeMicroseconds(LEFT_FOR); // sets the servo position delay(Dlay); } //************* TRightFast(int lVal,int Dlay) ****************** // moving right turn //************************************************************** void TRightFast(int lVal, int Dlay) { R_Servo.writeMicroseconds(RIGHT_FOR); // sets the servo position L_Servo.writeMicroseconds(lVal); // sets the servo position delay(Dlay); } //******************* DriveServosRC() ************************ //****** Use the value collected from Ch1 and Ch2 ************ //****** on a single stick to relatively calculate *********** //**** speed and direction of two servo driven wheels ********* //************************************************************** void DriveServosRC() { Ch1 = pulseIn(12, HIGH, 21000); // Capture pulse width on Channel 1 Ch2 = pulseIn(11, HIGH, 21000); // Capture pulse width on Channel 2 Ch3 = pulseIn(10, HIGH, 21000); // Capture pulse width on Channel 3 //Ch4 = pulseIn(9, HIGH, 21000); // Capture pulse width on Channel 4 //Ch6 = pulseIn(7, HIGH, 21000); // Capture pulse width on Channel 6 if (Ch2 <= 1500) { Lwheel = Ch1 + Ch2 - 1500; Rwheel = Ch1 - Ch2 + 1500; SetLimits(); } if (Ch2 > 1500) { int Ch1_mod = map(Ch1, 1000, 2000, 2000, 1000); // Invert the Ch1 axis Lwheel = Ch1_mod + Ch2 - 1500; Rwheel = Ch1_mod - Ch2 + 1500; SetLimits(); } Boom = Ch3; if (Ch3 > 1700) Boom = 1900; if (Ch3 < 1600) { Boom = 1000; SetLimits(); } //PrintRC(); //Print Values for RC Mode Diagnostics //checkLightSensors();//Un-comment to evaluate sensors //printSensors();//Un-comment to Display values } //********************** SetLimits() *************************** //******* Make sure values never exceed ranges *************** //****** For most all servos and like controllers ************ //**** control must fall between 1000uS and 2000uS ********** //************************************************************** void SetLimits() { // Can be set to a value you don't wish to exceed // to adjust maximums for your own robot if (Lwheel < 1000) Lwheel = 1000; if (Lwheel > 2000) Lwheel = 2000; if (Rwheel < 1000) Rwheel = 1000; if (Rwheel > 2000) Rwheel = 2000; if (Ch3 > 1900) Boom = 1900; if (Ch3 < 1600) Boom = 1600; pulseMotors(); } //******************* pulseMotors *************************** //pulses either mapped or direct signals generated from Mixlimits //************************************************************** void pulseMotors() { //un-comment the next two line to drive the wheels directly with the MaxLimits Set R_Servo.writeMicroseconds(Rwheel); L_Servo.writeMicroseconds(Lwheel); B_Servo.writeMicroseconds(Boom); //un-comment the next two to map a control range. //*** Take the standard range of 1000 to 2000 and frame it to your own minimum and maximum //*** for each wheel. // Rwheel = map(Rwheel, 1000, 2000, 1200, 1700); // Lwheel = map(Lwheel, 1000, 2000, 1200, 1700); // R_Servo.writeMicroseconds(Rwheel); // L_Servo.writeMicroseconds(Lwheel); // un-comment this line do display the value being sent to the motors // PrintWheelCalcs(); //REMEMBER: printing values slows reaction times } //****************** printLightSensors() ********************* // print the two light sensors and difference between them //************************************************************** void printLightSensors() { int valDiff = abs(lPhotoVal - rPhotoVal); Serial.print("lPhotoVal: " + (String)lPhotoVal + " rPhotoVal: " + (String)rPhotoVal); Serial.println(" valDiff: " + (String)valDiff); } //****************** printProxSensors() ********************* // print the 6 proximity sensor values //************************************************************** void printProxSensors() { Serial.print("proxFFL: " + (String)proxFFL + " proxFFR: " + (String)proxFFR); Serial.print(" proxFL: " + (String)proxFL + " proxFR: " + (String)proxFR); Serial.println(" proxBL: " + (String)proxBL + " proxBR: " + (String)proxBR); } //********************** PrintRC() *************************** //*** Simply print the collected RC values for diagnostics *** //************************************************************** void PrintRC() { // print out the values you read in: //Serial.println(" RC Control Mode "); //Serial.println(Ch1); //Serial.print("Value Ch2 = "); //Serial.println(Ch2); //Serial.print("Value Lwheel = "); //Serial.println(Lwheel); //Serial.print("Value Rwheel = "); //Serial.println(Rwheel); //Serial.print("Value Ch3 = "); //Serial.println(Ch3); //Serial.print("Value Ch4 = "); //Serial.println(Ch4); //Serial.print("Control = "); //Serial.println(Ch5); //Serial.print("Value Boom = "); //Serial.println(Boom); //Serial.println(" "); //delay(500); }