Justin Carr / theRobot

nav fixed

Dependencies:   Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

Fork of theRobot by Thomas Ashworth

main.cpp@10:1a1d52207f59, 2014-03-29 (annotated)

Committer:
tashworth
Date:
Sat Mar 29 21:22:14 2014 +0000
Revision:
10:1a1d52207f59
Parent:
9:1b29cd9ed1ba
Child:
11:8d2455e383ce
3-29-14 4:20pm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tashworth 0:1b64a0cedc5d 1 #include "mbed.h"
tashworth 0:1b64a0cedc5d 2 #include "Adafruit_PWMServoDriver.h"
tashworth 0:1b64a0cedc5d 3 #include "ShapeDetect.h"
tashworth 8:77a57909aa15 4 #include "rtos.h"
tashworth 8:77a57909aa15 5 #include "PID.h"
tashworth 8:77a57909aa15 6 #include "PololuQik2.h"
tashworth 8:77a57909aa15 7 #include "QEI.h"
tashworth 8:77a57909aa15 8 #include "HCSR04.h"
tashworth 8:77a57909aa15 9 #include "stdio.h"
tashworth 8:77a57909aa15 10 #include "LPC17xx.h"
tashworth 8:77a57909aa15 11 #include "Sharp.h"
tashworth 0:1b64a0cedc5d 12
tashworth 8:77a57909aa15 13 /* Navigation Definitions */
tashworth 8:77a57909aa15 14 #define PIN_TRIGGERL (p12)
tashworth 8:77a57909aa15 15 #define PIN_ECHOL (p11)
tashworth 8:77a57909aa15 16 #define PIN_TRIGGERR (p29)
tashworth 8:77a57909aa15 17 #define PIN_ECHOR (p30)
tashworth 8:77a57909aa15 18 #define PULSE_PER_REV (1192)
tashworth 8:77a57909aa15 19 #define WHEEL_CIRCUM (12.56637)
tashworth 8:77a57909aa15 20 #define DIST_PER_PULSE (0.01054225722682)
tashworth 8:77a57909aa15 21 #define MTRS_TO_INCH (39.3701)
tashworth 8:77a57909aa15 22 #define MAX_SPEED (0.3*127)
tashworth 8:77a57909aa15 23 #define PPR (4331/4)
tashworth 8:77a57909aa15 24 #define LEFT (1)
tashworth 8:77a57909aa15 25 #define RIGHT (0)
tashworth 8:77a57909aa15 26 #define FORWARD (1)
tashworth 8:77a57909aa15 27 #define BACKWARD (0)
tashworth 8:77a57909aa15 28 #define TOOLS (0)
tashworth 8:77a57909aa15 29 #define MID (1)
tashworth 8:77a57909aa15 30 #define RIGS (2)
tashworth 8:77a57909aa15 31 #define FIRST_WAVE (0)
tashworth 8:77a57909aa15 32 #define FAR (1)
tashworth 6:75259c3306dd 33
tashworth 6:75259c3306dd 34 //States
tashworth 6:75259c3306dd 35 #define START 0
tashworth 6:75259c3306dd 36 #define OILRIG1_POS 1
tashworth 6:75259c3306dd 37 #define OILRIG2_POS 2
tashworth 6:75259c3306dd 38 #define GOTO_TOOLS 3
tashworth 6:75259c3306dd 39 #define IDENTIFY_TOOLS 4
tashworth 6:75259c3306dd 40 #define AQUIRE_TOOL1 5
tashworth 6:75259c3306dd 41 #define AQUIRE_TOOL2 6
tashworth 6:75259c3306dd 42 #define AQUIRE_TOOL3 7
tashworth 6:75259c3306dd 43 #define NAVIGATE_WAVES_ROW1 8
tashworth 6:75259c3306dd 44 #define NAVIGATE_WAVES_ROW2 9
tashworth 6:75259c3306dd 45 #define NAVIGATE_WAVES_ROW3 10
tashworth 6:75259c3306dd 46 #define NAVIGATE_TO_SQUARE_RIG 11
tashworth 6:75259c3306dd 47 #define NAVIGATE_TO_TRIANGLE_RIG 12
tashworth 6:75259c3306dd 48 #define NAVIGATE_TO_CIRCLE_RIG 13
tashworth 6:75259c3306dd 49 #define RIG_ALIGN 14
tashworth 6:75259c3306dd 50 #define INSERT_TOOL 15
tashworth 6:75259c3306dd 51 #define END 16
tashworth 6:75259c3306dd 52
tashworth 0:1b64a0cedc5d 53
tashworth 8:77a57909aa15 54
tashworth 6:75259c3306dd 55 //Servo Static Positions
tashworth 6:75259c3306dd 56 #define STORE_POSITION 0
tashworth 6:75259c3306dd 57 #define OIL_RIG1 1
tashworth 6:75259c3306dd 58 #define OIL_RIG2 2
tashworth 6:75259c3306dd 59 #define OIL_RIG3 3
tashworth 6:75259c3306dd 60 #define DRIVE_POSITION_NOTOOL 4
tashworth 6:75259c3306dd 61 #define TOOL_1 5
tashworth 6:75259c3306dd 62 #define TOOL_2 6
tashworth 6:75259c3306dd 63 #define TOOL_3 7
tashworth 6:75259c3306dd 64 #define DRIVE_POSITION_TOOL 8
tashworth 6:75259c3306dd 65 #define ORIENT_TOOL 9
tashworth 2:4e082e4c255d 66
tashworth 6:75259c3306dd 67 //Rig definitions
tashworth 6:75259c3306dd 68 #define SQUARE 1
tashworth 6:75259c3306dd 69 #define TRIANGLE 2
tashworth 6:75259c3306dd 70 #define CIRCLE 3
tashworth 6:75259c3306dd 71
tashworth 6:75259c3306dd 72 //*********************//
tashworth 6:75259c3306dd 73 //******* TODO ********//
tashworth 6:75259c3306dd 74 //*********************//
tashworth 6:75259c3306dd 75 //Oil Rig distance thresholds
tashworth 8:77a57909aa15 76 #define OILRIG1_MAX 3000
tashworth 8:77a57909aa15 77 #define OILRIG1_MIN 1000
tashworth 9:1b29cd9ed1ba 78 #define OILRIG2_MAX 5000
tashworth 8:77a57909aa15 79 #define OILRIG2_MIN 1000
tashworth 9:1b29cd9ed1ba 80 #define OILRIG3_MAX 5000
tashworth 8:77a57909aa15 81 #define OILRIG3_MIN 1000
tashworth 6:75259c3306dd 82
tashworth 6:75259c3306dd 83 //for servo normalization
tashworth 3:b7b4780a7f6e 84 #define MIN_SERVO_PULSE 900
tashworth 3:b7b4780a7f6e 85 #define MAX_SERVO_PULSE 2100
tashworth 3:b7b4780a7f6e 86 #define SERVO_MAX_ANGLE 180
tashworth 3:b7b4780a7f6e 87
tashworth 8:77a57909aa15 88
tashworth 8:77a57909aa15 89 DigitalOut myled1(LED1);
tashworth 8:77a57909aa15 90 DigitalOut myled2(LED2);
tashworth 8:77a57909aa15 91 DigitalOut myled3(LED3);
tashworth 8:77a57909aa15 92 DigitalOut myled4(LED4);
tashworth 8:77a57909aa15 93
tashworth 8:77a57909aa15 94 void errFunction(void);
tashworth 8:77a57909aa15 95 bool cRc;
tashworth 8:77a57909aa15 96
tashworth 8:77a57909aa15 97 Serial pc(USBTX,USBRX); //USB Comm
tashworth 8:77a57909aa15 98 Adafruit_PWMServoDriver pwm(p28,p27); //pwm(SDA,SCL) - Servo Control PWM
tashworth 8:77a57909aa15 99 DigitalOut ServoOutputDisable(p8); //Servo Control Output Enable/Disable
tashworth 8:77a57909aa15 100 extern Serial lrf; //Laser Range Finder lrf(p13,p14)
tashworth 8:77a57909aa15 101 //Hardware Initialization
tashworth 8:77a57909aa15 102 //Serial bt(p13,p14);
tashworth 8:77a57909aa15 103 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
tashworth 8:77a57909aa15 104 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
tashworth 8:77a57909aa15 105 PID pid1(15.0,0.0,4.0,0.02);
tashworth 8:77a57909aa15 106 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
tashworth 8:77a57909aa15 107 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
tashworth 8:77a57909aa15 108 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
tashworth 8:77a57909aa15 109 Sharp IR(p20);
tashworth 8:77a57909aa15 110 //InterruptIn encoder(p29);
tashworth 8:77a57909aa15 111
tashworth 8:77a57909aa15 112
tashworth 8:77a57909aa15 113
tashworth 8:77a57909aa15 114
tashworth 6:75259c3306dd 115 /***************
tashworth 6:75259c3306dd 116 local servo functions
tashworth 6:75259c3306dd 117 ****************/
tashworth 1:fe4a0b47ff25 118 void servoBegin(void);
tashworth 0:1b64a0cedc5d 119 void initServoDriver(void);
tashworth 3:b7b4780a7f6e 120 void setServoPulse(uint8_t n, int angle);
tashworth 3:b7b4780a7f6e 121 void setServoPulseNo_delay(uint8_t n, int angle);
tashworth 0:1b64a0cedc5d 122 void servoPosition(int set);
tashworth 3:b7b4780a7f6e 123 void setGripper(int open);
tashworth 6:75259c3306dd 124 int fire_checker(int rig);
tashworth 6:75259c3306dd 125
tashworth 8:77a57909aa15 126
tashworth 8:77a57909aa15 127 //Navigation Functions
tashworth 8:77a57909aa15 128 float wall_follow(int side, int direction, int section);
tashworth 8:77a57909aa15 129 void wall_follow2(int side, int direction, int section, float location);
tashworth 8:77a57909aa15 130 void wall_follow3(int &currentLocation, int &WaveOpening);
tashworth 8:77a57909aa15 131 void leftTurn(void);
tashworth 8:77a57909aa15 132 void slightleft(void);
tashworth 8:77a57909aa15 133 void rightTurn(void);
tashworth 8:77a57909aa15 134 void us_distance(void);
tashworth 8:77a57909aa15 135 void to_tools_section(float* location, float &current);
tashworth 8:77a57909aa15 136 void from_tools_section(float* location, float &current);
tashworth 8:77a57909aa15 137 void mid_section(float* location, float &current, int* direction);
tashworth 8:77a57909aa15 138 void mid_section2(float* location, float &current, int* direction);
tashworth 8:77a57909aa15 139 void rig_section(float* location, float &current, int* direction, int rig);
tashworth 8:77a57909aa15 140 void overBump(int section);
tashworth 8:77a57909aa15 141 void alignWithWall(int section);
tashworth 8:77a57909aa15 142 void ledtoggle(void);
tashworth 8:77a57909aa15 143
tashworth 8:77a57909aa15 144
tashworth 6:75259c3306dd 145 /************
tashworth 6:75259c3306dd 146 Main Variables
tashworth 6:75259c3306dd 147 *************/
tashworth 6:75259c3306dd 148 int state = START;
tashworth 6:75259c3306dd 149 int fire = 0;
tashworth 6:75259c3306dd 150 int tool_needed = 0;
tashworth 6:75259c3306dd 151 int shape_detected = 0;
tashworth 8:77a57909aa15 152 float range, range2, pid_return;
tashworth 8:77a57909aa15 153
tashworth 0:1b64a0cedc5d 154
tashworth 0:1b64a0cedc5d 155 /************
tashworth 0:1b64a0cedc5d 156 Variables for Servos
tashworth 0:1b64a0cedc5d 157 *************/
tashworth 3:b7b4780a7f6e 158 int servoNum, servoAngle, outputDisabled, posNum, testVal;
tashworth 3:b7b4780a7f6e 159 int currentPosition[7];
tashworth 0:1b64a0cedc5d 160
tashworth 3:b7b4780a7f6e 161 typedef struct {
tashworth 6:75259c3306dd 162 int arm_position_name; //for organization only (STORE, OILRIG1, OILRIG2...)
tashworth 3:b7b4780a7f6e 163 int base_rotate;
tashworth 3:b7b4780a7f6e 164 int base_arm;
tashworth 3:b7b4780a7f6e 165 int big_arm;
tashworth 3:b7b4780a7f6e 166 int claw_arm;
tashworth 3:b7b4780a7f6e 167 int claw_rotate;
tashworth 3:b7b4780a7f6e 168 int claw_open;
tashworth 3:b7b4780a7f6e 169 } Coord;
tashworth 3:b7b4780a7f6e 170
tashworth 6:75259c3306dd 171 /********************
tashworth 6:75259c3306dd 172 Static Arm Positions
tashworth 6:75259c3306dd 173 *********************/
tashworth 6:75259c3306dd 174
tashworth 3:b7b4780a7f6e 175 Coord Arm_Table[] = {
tashworth 6:75259c3306dd 176
tashworth 3:b7b4780a7f6e 177 // POSITION ODER:
tashworth 6:75259c3306dd 178 // base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
tashworth 3:b7b4780a7f6e 179
tashworth 8:77a57909aa15 180 //increase in number 5 rotates gripper
tashworth 8:77a57909aa15 181
tashworth 9:1b29cd9ed1ba 182 {STORE_POSITION, 85, 10, 0, 175, 100, 0}, // storing position
tashworth 10:1a1d52207f59 183 {OIL_RIG1, 161, 90, 90, 47, 100, 0}, // point laser at oilrig1
tashworth 10:1a1d52207f59 184 {OIL_RIG2, 140, 90, 90, 44, 100, 0}, // point laser at oilrig2
tashworth 10:1a1d52207f59 185 {OIL_RIG3, 130, 90, 90, 57, 100, 0}, // point laser at oilrig2
tashworth 9:1b29cd9ed1ba 186 {DRIVE_POSITION_NOTOOL, 85, 10, 0, 175, 100, 0}, // Drive through course
tashworth 8:77a57909aa15 187 {TOOL_1, 95, 64, 97, 79, 0, 0}, // Look over first tool
tashworth 8:77a57909aa15 188 {TOOL_2, 75, 70, 102, 74, 0, 0}, // Look over second tool
tashworth 8:77a57909aa15 189 {TOOL_3, 55, 70, 102, 74, 0, 0}, // Look over third tool
tashworth 8:77a57909aa15 190 {DRIVE_POSITION_TOOL, 55, 70, 102, 74, 0, 0}, // Drive with tool loaded
tashworth 6:75259c3306dd 191 {ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
tashworth 3:b7b4780a7f6e 192 };
tashworth 3:b7b4780a7f6e 193
tashworth 3:b7b4780a7f6e 194
tashworth 3:b7b4780a7f6e 195 /* Variables for imageprocessing and distance */
tashworth 3:b7b4780a7f6e 196 int x_coord;
tashworth 3:b7b4780a7f6e 197 int y_coord;
tashworth 3:b7b4780a7f6e 198 int area;
tashworth 3:b7b4780a7f6e 199 int shape;
tashworth 3:b7b4780a7f6e 200
tashworth 6:75259c3306dd 201 /* Variables for distance sensor*/
tashworth 8:77a57909aa15 202 int distLaser;
tashworth 6:75259c3306dd 203 int fire_detected = 0;
tashworth 6:75259c3306dd 204 int fire_not_detected = 0;
tashworth 3:b7b4780a7f6e 205
tashworth 0:1b64a0cedc5d 206
tashworth 3:b7b4780a7f6e 207 int main()
tashworth 3:b7b4780a7f6e 208 {
tashworth 3:b7b4780a7f6e 209
tashworth 3:b7b4780a7f6e 210 /*****************
tashworth 3:b7b4780a7f6e 211 INITIALIZATIONS
tashworth 3:b7b4780a7f6e 212 *******************/
tashworth 10:1a1d52207f59 213 float location[3], current=6;
tashworth 8:77a57909aa15 214 int direction[3];
tashworth 8:77a57909aa15 215 double distance;
tashworth 8:77a57909aa15 216
tashworth 9:1b29cd9ed1ba 217
tashworth 3:b7b4780a7f6e 218 pc.baud(115200);
tashworth 6:75259c3306dd 219 //Laser Range Finder Initialization
tashworth 3:b7b4780a7f6e 220 lrf_baudCalibration();
tashworth 8:77a57909aa15 221 motors.begin();
tashworth 3:b7b4780a7f6e 222
tashworth 6:75259c3306dd 223 //Servo initialization
tashworth 3:b7b4780a7f6e 224 initServoDriver();
tashworth 6:75259c3306dd 225 servoBegin(); //initiates servos to start position
tashworth 10:1a1d52207f59 226 //servoPosition(0);
tashworth 10:1a1d52207f59 227 ServoOutputDisable = 0;
tashworth 6:75259c3306dd 228
tashworth 6:75259c3306dd 229 /*******************
tashworth 6:75259c3306dd 230 WHILE LOOP FOR TESTING
tashworth 6:75259c3306dd 231 ********************/
tashworth 10:1a1d52207f59 232 while(1) {
tashworth 10:1a1d52207f59 233
tashworth 8:77a57909aa15 234 pc.scanf("%d %d", &servoNum, &servoAngle);
tashworth 8:77a57909aa15 235 if(servoAngle > 175) {
tashworth 8:77a57909aa15 236 servoAngle = 175;
tashworth 8:77a57909aa15 237 }
tashworth 8:77a57909aa15 238 if(servoNum > 5 ) {
tashworth 8:77a57909aa15 239 servoNum = 0;
tashworth 8:77a57909aa15 240 servoAngle = 90;
tashworth 8:77a57909aa15 241 }
tashworth 8:77a57909aa15 242 setServoPulse(servoNum, servoAngle);
tashworth 6:75259c3306dd 243
tashworth 8:77a57909aa15 244 //shape_detected = shapeDetection();
tashworth 8:77a57909aa15 245 //distLaser = getDistance();
tashworth 8:77a57909aa15 246 //pc.printf("Distance %d", distLaser);
tashworth 9:1b29cd9ed1ba 247 //ledtoggle();
tashworth 8:77a57909aa15 248
tashworth 10:1a1d52207f59 249 //pc.scanf("%d", &posNum);
tashworth 10:1a1d52207f59 250 //servoPosition(posNum);
tashworth 10:1a1d52207f59 251 //wait(5);
tashworth 9:1b29cd9ed1ba 252 //shape_detected = shapeDetection();
tashworth 9:1b29cd9ed1ba 253 distLaser = getDistance();
tashworth 9:1b29cd9ed1ba 254 pc.printf("Distance %d", distLaser);
tashworth 9:1b29cd9ed1ba 255
tashworth 10:1a1d52207f59 256 }
tashworth 3:b7b4780a7f6e 257
tashworth 7:8fb4204f9600 258 /********************************
tashworth 7:8fb4204f9600 259 MAIN WHILE LOOP FOR COMPETITION
tashworth 7:8fb4204f9600 260 *********************************/
tashworth 3:b7b4780a7f6e 261 while(1) {
tashworth 10:1a1d52207f59 262
tashworth 6:75259c3306dd 263 switch (state) {
tashworth 8:77a57909aa15 264
tashworth 7:8fb4204f9600 265 /**************************************************
tashworth 7:8fb4204f9600 266 * STAGE 0
tashworth 7:8fb4204f9600 267 *
tashworth 7:8fb4204f9600 268 * - START OF THE COMETITION
tashworth 7:8fb4204f9600 269 *
tashworth 7:8fb4204f9600 270 **************************************************/
tashworth 6:75259c3306dd 271 case START :
tashworth 6:75259c3306dd 272 myled1 = 1;
tashworth 10:1a1d52207f59 273 wait(1);
tashworth 6:75259c3306dd 274 myled1 = 0;
tashworth 6:75259c3306dd 275 state = OILRIG1_POS;
tashworth 3:b7b4780a7f6e 276 break;
tashworth 3:b7b4780a7f6e 277
tashworth 6:75259c3306dd 278 /**************************************************
tashworth 6:75259c3306dd 279 * STAGE 1
tashworth 6:75259c3306dd 280 *
tashworth 6:75259c3306dd 281 * - DETERMINE OIL RIG ON FIRE
tashworth 6:75259c3306dd 282 *
tashworth 6:75259c3306dd 283 **************************************************/
tashworth 6:75259c3306dd 284 case OILRIG1_POS: //aims arm at square oil rig
tashworth 10:1a1d52207f59 285
tashworth 10:1a1d52207f59 286 setServoPulse(3, Arm_Table[OIL_RIG1].claw_arm);
tashworth 10:1a1d52207f59 287 setServoPulse(2, 50);
tashworth 10:1a1d52207f59 288 setServoPulse(1, Arm_Table[OIL_RIG1].base_arm);
tashworth 10:1a1d52207f59 289 setServoPulse(2, Arm_Table[OIL_RIG1].big_arm);
tashworth 10:1a1d52207f59 290 setServoPulse(0, Arm_Table[OIL_RIG1].base_rotate);
tashworth 10:1a1d52207f59 291 setServoPulse(4, Arm_Table[OIL_RIG1].claw_rotate);
tashworth 10:1a1d52207f59 292 setServoPulse(5, Arm_Table[OIL_RIG1].claw_open);
tashworth 10:1a1d52207f59 293
tashworth 10:1a1d52207f59 294 wait(3); //wait for servos to settle
tashworth 10:1a1d52207f59 295
tashworth 6:75259c3306dd 296 fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
tashworth 10:1a1d52207f59 297 pc.printf("FIRE: %d", fire);
tashworth 6:75259c3306dd 298 //determines what tool is needed
tashworth 6:75259c3306dd 299 if (fire == 1) {
tashworth 6:75259c3306dd 300 tool_needed = SQUARE;
tashworth 6:75259c3306dd 301 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 302 } else {
tashworth 6:75259c3306dd 303 state = OILRIG2_POS;
tashworth 6:75259c3306dd 304 }
tashworth 10:1a1d52207f59 305
tashworth 6:75259c3306dd 306 break;
tashworth 6:75259c3306dd 307
tashworth 6:75259c3306dd 308 case OILRIG2_POS:
tashworth 6:75259c3306dd 309 servoPosition(OIL_RIG2); //position arm to point at first oilrig
tashworth 6:75259c3306dd 310 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 311 fire = fire_checker(OIL_RIG2);
tashworth 10:1a1d52207f59 312 pc.printf("FIRE: %d", fire);
tashworth 6:75259c3306dd 313 if (fire == 1) {
tashworth 6:75259c3306dd 314 tool_needed = TRIANGLE;
tashworth 6:75259c3306dd 315 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 316 } else {
tashworth 6:75259c3306dd 317 tool_needed = CIRCLE;
tashworth 6:75259c3306dd 318 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 319 }
tashworth 9:1b29cd9ed1ba 320 pc.printf("tool needed: %d", tool_needed);
tashworth 6:75259c3306dd 321 break;
tashworth 6:75259c3306dd 322
tashworth 6:75259c3306dd 323 /**************************************************
tashworth 6:75259c3306dd 324 * STAGE 2
tashworth 6:75259c3306dd 325 *
tashworth 6:75259c3306dd 326 * - TRAVEL TO TOOLS
tashworth 6:75259c3306dd 327 *
tashworth 6:75259c3306dd 328 **************************************************/
tashworth 6:75259c3306dd 329 case GOTO_TOOLS:
tashworth 10:1a1d52207f59 330
tashworth 10:1a1d52207f59 331 setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
tashworth 10:1a1d52207f59 332 setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
tashworth 10:1a1d52207f59 333 setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
tashworth 10:1a1d52207f59 334 setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
tashworth 10:1a1d52207f59 335 setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
tashworth 10:1a1d52207f59 336 setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
tashworth 10:1a1d52207f59 337
tashworth 10:1a1d52207f59 338 wait(5); //wait for servos to settle
tashworth 8:77a57909aa15 339
tashworth 8:77a57909aa15 340 to_tools_section(location, current);
tashworth 6:75259c3306dd 341
tashworth 6:75259c3306dd 342 state = IDENTIFY_TOOLS;
tashworth 9:1b29cd9ed1ba 343 pc.printf("YES!!!!!");
tashworth 9:1b29cd9ed1ba 344 while(1);
tashworth 6:75259c3306dd 345 break;
tashworth 6:75259c3306dd 346 /**************************************************
tashworth 6:75259c3306dd 347 * STAGE 3
tashworth 6:75259c3306dd 348 *
tashworth 6:75259c3306dd 349 * - Determine order of tools
tashworth 6:75259c3306dd 350 * - Aquire appropriate tool
tashworth 6:75259c3306dd 351 *
tashworth 6:75259c3306dd 352 **************************************************/
tashworth 6:75259c3306dd 353 case IDENTIFY_TOOLS:
tashworth 6:75259c3306dd 354 servoPosition(TOOL_1); //arm/camera looks over tool
tashworth 6:75259c3306dd 355 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 356 centerCamWithTool(); //centers camera over tool
tashworth 6:75259c3306dd 357 shape_detected = shapeDetection(); //determines the shape
tashworth 3:b7b4780a7f6e 358
tashworth 6:75259c3306dd 359 //either goes to aquire the tool or look at the next shape
tashworth 6:75259c3306dd 360 if(shape_detected == tool_needed) {
tashworth 6:75259c3306dd 361 state = AQUIRE_TOOL1;
tashworth 6:75259c3306dd 362 } else {
tashworth 6:75259c3306dd 363 servoPosition(TOOL_2);
tashworth 6:75259c3306dd 364 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 365 centerCamWithTool();
tashworth 6:75259c3306dd 366 shape_detected = shapeDetection();
tashworth 6:75259c3306dd 367 if (shape_detected == tool_needed) {
tashworth 6:75259c3306dd 368 state = AQUIRE_TOOL2;
tashworth 6:75259c3306dd 369 } else {
tashworth 6:75259c3306dd 370 servoPosition(TOOL_3);
tashworth 6:75259c3306dd 371 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 372 centerCamWithTool();
tashworth 6:75259c3306dd 373 state = AQUIRE_TOOL3;
tashworth 6:75259c3306dd 374 }
tashworth 6:75259c3306dd 375 }
tashworth 6:75259c3306dd 376 break;
tashworth 6:75259c3306dd 377 case AQUIRE_TOOL1:
tashworth 6:75259c3306dd 378 //*********************//
tashworth 6:75259c3306dd 379 //******* TODO ********//
tashworth 6:75259c3306dd 380 //*********************//
tashworth 6:75259c3306dd 381 // CODE TO GRAB TOOL1
tashworth 6:75259c3306dd 382 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 383 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 384 break;
tashworth 6:75259c3306dd 385 case AQUIRE_TOOL2:
tashworth 6:75259c3306dd 386 //*********************//
tashworth 6:75259c3306dd 387 //******* TODO ********//
tashworth 6:75259c3306dd 388 //*********************//
tashworth 6:75259c3306dd 389 // CODE TO GRAB TOOL2
tashworth 6:75259c3306dd 390 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 391 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 392 break;
tashworth 6:75259c3306dd 393 case AQUIRE_TOOL3:
tashworth 6:75259c3306dd 394 //*********************//
tashworth 6:75259c3306dd 395 //******* TODO ********//
tashworth 6:75259c3306dd 396 //*********************//
tashworth 6:75259c3306dd 397 // CODE TO GRAB TOOL3
tashworth 6:75259c3306dd 398 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 399 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 400 break;
tashworth 3:b7b4780a7f6e 401
tashworth 0:1b64a0cedc5d 402
tashworth 6:75259c3306dd 403 /**************************************************
tashworth 6:75259c3306dd 404 * STAGE 4
tashworth 6:75259c3306dd 405 *
tashworth 6:75259c3306dd 406 * - Navigate through the ocean
tashworth 6:75259c3306dd 407 *
tashworth 6:75259c3306dd 408 **************************************************/
tashworth 6:75259c3306dd 409
tashworth 6:75259c3306dd 410 case NAVIGATE_WAVES_ROW1:
tashworth 6:75259c3306dd 411 //*********************//
tashworth 6:75259c3306dd 412 //******* TODO ********//
tashworth 6:75259c3306dd 413 //*********************//
tashworth 6:75259c3306dd 414 // CODE TO NAVIGATE ROW1
tashworth 6:75259c3306dd 415 state = NAVIGATE_WAVES_ROW2;
tashworth 6:75259c3306dd 416 break;
tashworth 6:75259c3306dd 417
tashworth 6:75259c3306dd 418 case NAVIGATE_WAVES_ROW2:
tashworth 6:75259c3306dd 419 //*********************//
tashworth 6:75259c3306dd 420 //******* TODO ********//
tashworth 6:75259c3306dd 421 //*********************//
tashworth 6:75259c3306dd 422 // CODE TO NAVIGATE ROW2
tashworth 6:75259c3306dd 423 state = NAVIGATE_WAVES_ROW3;
tashworth 6:75259c3306dd 424 break;
tashworth 3:b7b4780a7f6e 425
tashworth 6:75259c3306dd 426 case NAVIGATE_WAVES_ROW3:
tashworth 6:75259c3306dd 427 //*********************//
tashworth 6:75259c3306dd 428 //******* TODO ********//
tashworth 6:75259c3306dd 429 //*********************//
tashworth 6:75259c3306dd 430 // CODE TO NAVIGATE ROW3
tashworth 7:8fb4204f9600 431
tashworth 6:75259c3306dd 432 //goes to appropriate rig
tashworth 6:75259c3306dd 433 if(shape_detected == 1) {
tashworth 6:75259c3306dd 434 state = NAVIGATE_TO_SQUARE_RIG;
tashworth 6:75259c3306dd 435 } else if(shape_detected == 2) {
tashworth 6:75259c3306dd 436 state = NAVIGATE_TO_TRIANGLE_RIG;
tashworth 6:75259c3306dd 437 } else {
tashworth 6:75259c3306dd 438 state = NAVIGATE_TO_CIRCLE_RIG;
tashworth 6:75259c3306dd 439 }
tashworth 6:75259c3306dd 440 break;
tashworth 6:75259c3306dd 441
tashworth 6:75259c3306dd 442 /**************************************************
tashworth 6:75259c3306dd 443 * STAGE 5
tashworth 6:75259c3306dd 444 *
tashworth 6:75259c3306dd 445 * - Travel to appropriate rig
tashworth 6:75259c3306dd 446 *
tashworth 6:75259c3306dd 447 **************************************************/
tashworth 6:75259c3306dd 448 case NAVIGATE_TO_SQUARE_RIG:
tashworth 6:75259c3306dd 449 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 450 state = RIG_ALIGN;
tashworth 6:75259c3306dd 451 break;
tashworth 6:75259c3306dd 452 case NAVIGATE_TO_TRIANGLE_RIG:
tashworth 6:75259c3306dd 453 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 454 state = RIG_ALIGN;
tashworth 6:75259c3306dd 455 break;
tashworth 6:75259c3306dd 456 case NAVIGATE_TO_CIRCLE_RIG:
tashworth 6:75259c3306dd 457 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 458 state = RIG_ALIGN;
tashworth 6:75259c3306dd 459 break;
tashworth 3:b7b4780a7f6e 460
tashworth 6:75259c3306dd 461 /**************************************************
tashworth 6:75259c3306dd 462 * STAGE 6
tashworth 6:75259c3306dd 463 *
tashworth 6:75259c3306dd 464 * - Align with appropriate rig
tashworth 6:75259c3306dd 465 *
tashworth 6:75259c3306dd 466 **************************************************/
tashworth 6:75259c3306dd 467 case RIG_ALIGN:
tashworth 7:8fb4204f9600 468
tashworth 6:75259c3306dd 469 //*********************//
tashworth 6:75259c3306dd 470 //******* TODO ********//
tashworth 6:75259c3306dd 471 //*********************//
tashworth 6:75259c3306dd 472 // CODE TO ALIGN ROBOT WITH RIG
tashworth 7:8fb4204f9600 473
tashworth 6:75259c3306dd 474 servoPosition(ORIENT_TOOL);
tashworth 6:75259c3306dd 475 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 476 state = INSERT_TOOL;
tashworth 6:75259c3306dd 477 break;
tashworth 3:b7b4780a7f6e 478
tashworth 6:75259c3306dd 479 /**************************************************
tashworth 6:75259c3306dd 480 * STAGE 7
tashworth 6:75259c3306dd 481 *
tashworth 6:75259c3306dd 482 * - Insert Tool
tashworth 6:75259c3306dd 483 * - Extenguish fire
tashworth 6:75259c3306dd 484 * - win contest
tashworth 6:75259c3306dd 485 *
tashworth 6:75259c3306dd 486 **************************************************/
tashworth 7:8fb4204f9600 487
tashworth 6:75259c3306dd 488 case INSERT_TOOL:
tashworth 6:75259c3306dd 489 //*********************//
tashworth 6:75259c3306dd 490 //******* TODO ********//
tashworth 6:75259c3306dd 491 //*********************//
tashworth 6:75259c3306dd 492 // CODE TO INSERT TOOL
tashworth 6:75259c3306dd 493 break;
tashworth 3:b7b4780a7f6e 494
tashworth 6:75259c3306dd 495 /**************************************************
tashworth 6:75259c3306dd 496 * STAGE 8
tashworth 6:75259c3306dd 497 *
tashworth 6:75259c3306dd 498 * - END COMPETITION
tashworth 6:75259c3306dd 499 *
tashworth 6:75259c3306dd 500 **************************************************/
tashworth 6:75259c3306dd 501 case END:
tashworth 6:75259c3306dd 502 servoPosition(STORE_POSITION);
tashworth 6:75259c3306dd 503 myled1 = 1;
tashworth 6:75259c3306dd 504 wait(.2);
tashworth 6:75259c3306dd 505 myled2 = 1;
tashworth 6:75259c3306dd 506 wait(.2);
tashworth 6:75259c3306dd 507 myled3 = 1;
tashworth 6:75259c3306dd 508 wait(.2);
tashworth 6:75259c3306dd 509 myled4 = 1;
tashworth 6:75259c3306dd 510 wait(.2);
tashworth 6:75259c3306dd 511 break;
tashworth 6:75259c3306dd 512 default:
tashworth 3:b7b4780a7f6e 513
tashworth 6:75259c3306dd 514 break;
tashworth 6:75259c3306dd 515 }
tashworth 6:75259c3306dd 516 }
tashworth 6:75259c3306dd 517
tashworth 0:1b64a0cedc5d 518
tashworth 0:1b64a0cedc5d 519 }
tashworth 0:1b64a0cedc5d 520
tashworth 0:1b64a0cedc5d 521
tashworth 0:1b64a0cedc5d 522
tashworth 0:1b64a0cedc5d 523 /************
tashworth 0:1b64a0cedc5d 524
tashworth 0:1b64a0cedc5d 525 Servo Functions
tashworth 0:1b64a0cedc5d 526
tashworth 0:1b64a0cedc5d 527 **************/
tashworth 0:1b64a0cedc5d 528
tashworth 3:b7b4780a7f6e 529 void setServoPulse(uint8_t n, int angle)
tashworth 3:b7b4780a7f6e 530 {
tashworth 3:b7b4780a7f6e 531 int pulse;
tashworth 3:b7b4780a7f6e 532 pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 1:fe4a0b47ff25 533 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 534 int i = currentPosition[n];
tashworth 3:b7b4780a7f6e 535 pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
tashworth 3:b7b4780a7f6e 536 int pulse2;
tashworth 3:b7b4780a7f6e 537 if(currentPosition[n] < pulse) {
tashworth 3:b7b4780a7f6e 538 for(i; i < pulse; i++) {
tashworth 3:b7b4780a7f6e 539 pulse2 = 4094 * i / pulselength;
tashworth 3:b7b4780a7f6e 540 pwm.setPWM(n, 0, pulse2);
tashworth 3:b7b4780a7f6e 541 wait_ms(3);
tashworth 3:b7b4780a7f6e 542 }
tashworth 1:fe4a0b47ff25 543 } else if (currentPosition[n] > pulse) {
tashworth 3:b7b4780a7f6e 544 for(i; i > pulse; i--) {
tashworth 3:b7b4780a7f6e 545 pulse2 = 4094 * i / pulselength;
tashworth 3:b7b4780a7f6e 546 pwm.setPWM(n, 0, pulse2);
tashworth 3:b7b4780a7f6e 547 wait_ms(3);
tashworth 3:b7b4780a7f6e 548 }
tashworth 1:fe4a0b47ff25 549 }
tashworth 1:fe4a0b47ff25 550 currentPosition[n] = i;
tashworth 3:b7b4780a7f6e 551 pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle);
tashworth 0:1b64a0cedc5d 552 }
tashworth 0:1b64a0cedc5d 553
tashworth 3:b7b4780a7f6e 554 void initServoDriver(void)
tashworth 3:b7b4780a7f6e 555 {
tashworth 0:1b64a0cedc5d 556 pwm.begin();
tashworth 0:1b64a0cedc5d 557 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 0:1b64a0cedc5d 558 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 0:1b64a0cedc5d 559 pwm.setI2Cfreq(400000); //400kHz
tashworth 3:b7b4780a7f6e 560
tashworth 3:b7b4780a7f6e 561 }
tashworth 3:b7b4780a7f6e 562
tashworth 3:b7b4780a7f6e 563 void servoBegin(void)
tashworth 3:b7b4780a7f6e 564 {
tashworth 3:b7b4780a7f6e 565 pc.printf("Setting Initial Position\n\r");
tashworth 8:77a57909aa15 566 setServoPulseNo_delay(3, 175);
tashworth 8:77a57909aa15 567 setServoPulseNo_delay(2, 0);
tashworth 10:1a1d52207f59 568 setServoPulseNo_delay(1, 2);
tashworth 8:77a57909aa15 569 setServoPulseNo_delay(0, 85);
tashworth 8:77a57909aa15 570 setServoPulseNo_delay(4, 100);
tashworth 8:77a57909aa15 571 setServoPulseNo_delay(5, 0);
tashworth 3:b7b4780a7f6e 572 setGripper(1);
tashworth 0:1b64a0cedc5d 573 }
tashworth 0:1b64a0cedc5d 574
tashworth 3:b7b4780a7f6e 575 void setServoPulseNo_delay(uint8_t n, int angle)
tashworth 3:b7b4780a7f6e 576 {
tashworth 3:b7b4780a7f6e 577 int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 1:fe4a0b47ff25 578 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 579 currentPosition[n] = pulse;
tashworth 8:77a57909aa15 580 //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
tashworth 1:fe4a0b47ff25 581 pulse = 4094 * pulse / pulselength;
tashworth 1:fe4a0b47ff25 582 pwm.setPWM(n, 0, pulse);
tashworth 3:b7b4780a7f6e 583
tashworth 1:fe4a0b47ff25 584 }
tashworth 3:b7b4780a7f6e 585
tashworth 3:b7b4780a7f6e 586 void setGripper(int open)
tashworth 3:b7b4780a7f6e 587 {
tashworth 3:b7b4780a7f6e 588 if (open) {
tashworth 3:b7b4780a7f6e 589 pc.printf("Gripper Open\r");
tashworth 3:b7b4780a7f6e 590 setServoPulseNo_delay(6, 10);
tashworth 3:b7b4780a7f6e 591 } else {
tashworth 3:b7b4780a7f6e 592 pc.printf("Gripper Closed\n\r");
tashworth 3:b7b4780a7f6e 593 setServoPulseNo_delay(6, 170);
tashworth 3:b7b4780a7f6e 594 }
tashworth 3:b7b4780a7f6e 595 }
tashworth 3:b7b4780a7f6e 596
tashworth 3:b7b4780a7f6e 597 void servoPosition(int set)
tashworth 3:b7b4780a7f6e 598 {
tashworth 3:b7b4780a7f6e 599 //moves to current position
tashworth 8:77a57909aa15 600 setServoPulse(3, Arm_Table[set].claw_arm);
tashworth 3:b7b4780a7f6e 601 setServoPulse(1, Arm_Table[set].base_arm);
tashworth 6:75259c3306dd 602 setServoPulse(2, Arm_Table[set].big_arm);
tashworth 8:77a57909aa15 603 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 6:75259c3306dd 604 setServoPulse(4, Arm_Table[set].claw_rotate);
tashworth 6:75259c3306dd 605 setServoPulse(5, Arm_Table[set].claw_open);
tashworth 6:75259c3306dd 606 }
tashworth 6:75259c3306dd 607
tashworth 8:77a57909aa15 608
tashworth 6:75259c3306dd 609 int fire_checker(int rig)
tashworth 6:75259c3306dd 610 {
tashworth 6:75259c3306dd 611 switch (rig) {
tashworth 6:75259c3306dd 612
tashworth 6:75259c3306dd 613 case 1:
tashworth 6:75259c3306dd 614 for (int i = 0; i<5; i++) {
tashworth 8:77a57909aa15 615 distLaser = getDistance();
tashworth 10:1a1d52207f59 616 pc.printf("L DISTANCE: %d \n\r", distLaser);
tashworth 8:77a57909aa15 617 if ((distLaser < OILRIG1_MAX)
tashworth 9:1b29cd9ed1ba 618 && (distLaser > OILRIG1_MIN)) {
tashworth 6:75259c3306dd 619 fire_detected++;
tashworth 6:75259c3306dd 620 } else {
tashworth 6:75259c3306dd 621 fire_not_detected++;
tashworth 6:75259c3306dd 622 }
tashworth 6:75259c3306dd 623 }
tashworth 9:1b29cd9ed1ba 624 break;
tashworth 6:75259c3306dd 625 case 2:
tashworth 6:75259c3306dd 626 for (int i = 0; i<5; i++) {
tashworth 8:77a57909aa15 627 distLaser = getDistance();
tashworth 10:1a1d52207f59 628 pc.printf("L DISTANCE: %d \n\r", distLaser);
tashworth 8:77a57909aa15 629 if ((distLaser < OILRIG2_MAX)
tashworth 9:1b29cd9ed1ba 630 && (distLaser > OILRIG2_MIN)) {
tashworth 6:75259c3306dd 631 fire_detected++;
tashworth 6:75259c3306dd 632 } else {
tashworth 6:75259c3306dd 633 fire_not_detected++;
tashworth 6:75259c3306dd 634 }
tashworth 6:75259c3306dd 635 }
tashworth 9:1b29cd9ed1ba 636 break;
tashworth 6:75259c3306dd 637 case 3:
tashworth 6:75259c3306dd 638 for (int i = 0; i<5; i++) {
tashworth 8:77a57909aa15 639 distLaser = getDistance();
tashworth 8:77a57909aa15 640 if ((distLaser < OILRIG3_MAX)
tashworth 9:1b29cd9ed1ba 641 && (distLaser > OILRIG3_MIN)) {
tashworth 6:75259c3306dd 642 fire_detected++;
tashworth 6:75259c3306dd 643 } else {
tashworth 6:75259c3306dd 644 fire_not_detected++;
tashworth 6:75259c3306dd 645 }
tashworth 6:75259c3306dd 646 }
tashworth 9:1b29cd9ed1ba 647 break;
tashworth 10:1a1d52207f59 648
tashworth 6:75259c3306dd 649 default:
tashworth 6:75259c3306dd 650 for (int i = 0; i<5; i++) {
tashworth 8:77a57909aa15 651 distLaser = getDistance();
tashworth 8:77a57909aa15 652 if ((distLaser < OILRIG1_MAX)
tashworth 10:1a1d52207f59 653 && (distLaser > OILRIG1_MIN)) {
tashworth 6:75259c3306dd 654 fire_detected++;
tashworth 6:75259c3306dd 655 } else {
tashworth 6:75259c3306dd 656 fire_not_detected++;
tashworth 6:75259c3306dd 657 }
tashworth 6:75259c3306dd 658 }
tashworth 9:1b29cd9ed1ba 659 break;
tashworth 6:75259c3306dd 660 }
tashworth 6:75259c3306dd 661
tashworth 6:75259c3306dd 662 if (fire_detected > fire_not_detected) {
tashworth 6:75259c3306dd 663 return 1;
tashworth 6:75259c3306dd 664 } else {
tashworth 6:75259c3306dd 665 return 0;
tashworth 6:75259c3306dd 666 }
tashworth 0:1b64a0cedc5d 667 }
tashworth 0:1b64a0cedc5d 668
tashworth 8:77a57909aa15 669 void errFunction(void)
tashworth 8:77a57909aa15 670 {
tashworth 8:77a57909aa15 671 //Nothing
tashworth 8:77a57909aa15 672 }
tashworth 8:77a57909aa15 673
tashworth 8:77a57909aa15 674 void us_distance(void)
tashworth 8:77a57909aa15 675 {
tashworth 8:77a57909aa15 676 pc.printf("Ultra Sonic\n\r");
tashworth 8:77a57909aa15 677 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 678 wait_us(20);
tashworth 8:77a57909aa15 679 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
tashworth 8:77a57909aa15 680 pc.printf("Range = %f\n\r", range);
tashworth 8:77a57909aa15 681 }
tashworth 8:77a57909aa15 682 }
tashworth 8:77a57909aa15 683
tashworth 8:77a57909aa15 684 float wall_follow(int side, int direction, int section)
tashworth 8:77a57909aa15 685 {
tashworth 8:77a57909aa15 686 float location, wavegap=0, set=5;
tashworth 8:77a57909aa15 687 int dir=1;
tashworth 8:77a57909aa15 688
tashworth 8:77a57909aa15 689 pid1.reset();
tashworth 8:77a57909aa15 690
tashworth 8:77a57909aa15 691 if(direction == BACKWARD) dir=-1;
tashworth 8:77a57909aa15 692 if(section == TOOLS)set= 10;
tashworth 8:77a57909aa15 693
tashworth 8:77a57909aa15 694 leftEncoder.reset();
tashworth 8:77a57909aa15 695 rightEncoder.reset();
tashworth 8:77a57909aa15 696
tashworth 8:77a57909aa15 697 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 698
tashworth 10:1a1d52207f59 699 while(location< 70) {
tashworth 8:77a57909aa15 700 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 701
tashworth 8:77a57909aa15 702 pid1.setInputLimits(0, set);
tashworth 8:77a57909aa15 703 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
tashworth 8:77a57909aa15 704 pid1.setSetPoint(set);
tashworth 8:77a57909aa15 705 if(side) {
tashworth 8:77a57909aa15 706 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 707 wait_ms(38);
tashworth 8:77a57909aa15 708 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 709 } else {
tashworth 8:77a57909aa15 710 rangeFinderRight.startMeas();
tashworth 8:77a57909aa15 711 wait_ms(38);
tashworth 8:77a57909aa15 712 rangeFinderRight.getMeas(range);
tashworth 8:77a57909aa15 713 pc.printf("%d\r\n",range);
tashworth 8:77a57909aa15 714 }
tashworth 8:77a57909aa15 715
tashworth 8:77a57909aa15 716 if(range > 20) {
tashworth 8:77a57909aa15 717 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 718 //pc.printf("wavegap %f\r\n",wavegap);
tashworth 8:77a57909aa15 719 // AT WAVE OPENING!!!!
tashworth 8:77a57909aa15 720 motors.setMotor1Speed(dir*0.4*127);//left
tashworth 8:77a57909aa15 721 motors.setMotor0Speed(dir*0.4*127);//right
tashworth 8:77a57909aa15 722 } else {
tashworth 8:77a57909aa15 723
tashworth 8:77a57909aa15 724 pid1.setProcessValue(range);
tashworth 8:77a57909aa15 725 pid_return = pid1.compute();
tashworth 8:77a57909aa15 726
tashworth 8:77a57909aa15 727 if(pid_return > 0) {
tashworth 8:77a57909aa15 728 if(side) {
tashworth 8:77a57909aa15 729 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
tashworth 8:77a57909aa15 730 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 731 } else {
tashworth 8:77a57909aa15 732 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
tashworth 8:77a57909aa15 733 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 734 }
tashworth 8:77a57909aa15 735 } else if(pid_return < 0) {
tashworth 8:77a57909aa15 736 if(side) {
tashworth 8:77a57909aa15 737 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 738 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
tashworth 8:77a57909aa15 739 } else {
tashworth 8:77a57909aa15 740 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 741 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
tashworth 8:77a57909aa15 742 }
tashworth 8:77a57909aa15 743 } else {
tashworth 8:77a57909aa15 744 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 745 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 746 }
tashworth 8:77a57909aa15 747 }
tashworth 8:77a57909aa15 748 }
tashworth 8:77a57909aa15 749 return wavegap;
tashworth 8:77a57909aa15 750 }
tashworth 8:77a57909aa15 751
tashworth 8:77a57909aa15 752 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
tashworth 8:77a57909aa15 753
tashworth 8:77a57909aa15 754 void wall_follow2(int side, int direction, int section, float location)
tashworth 8:77a57909aa15 755 {
tashworth 8:77a57909aa15 756 int SeeWaveGap = false, dir=1;
tashworth 8:77a57909aa15 757 float set=5, loc=0;
tashworth 8:77a57909aa15 758
tashworth 8:77a57909aa15 759 pid1.reset();
tashworth 8:77a57909aa15 760
tashworth 8:77a57909aa15 761 if(direction == BACKWARD) dir=-1;
tashworth 8:77a57909aa15 762 if(section == TOOLS)set= 5;
tashworth 8:77a57909aa15 763
tashworth 8:77a57909aa15 764 leftEncoder.reset();
tashworth 8:77a57909aa15 765 rightEncoder.reset();
tashworth 8:77a57909aa15 766
tashworth 8:77a57909aa15 767 while(dir*loc + location <= 78) {
tashworth 8:77a57909aa15 768 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 769
tashworth 8:77a57909aa15 770 pid1.setInputLimits(0.0, set);
tashworth 8:77a57909aa15 771 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
tashworth 8:77a57909aa15 772 pid1.setSetPoint(set);
tashworth 8:77a57909aa15 773
tashworth 8:77a57909aa15 774 if(side) {
tashworth 8:77a57909aa15 775 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 776 wait_ms(38);
tashworth 8:77a57909aa15 777 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 778 } else {
tashworth 8:77a57909aa15 779 rangeFinderRight.startMeas();
tashworth 8:77a57909aa15 780 wait_ms(38);
tashworth 8:77a57909aa15 781 rangeFinderRight.getMeas(range);
tashworth 8:77a57909aa15 782 }
tashworth 8:77a57909aa15 783
tashworth 8:77a57909aa15 784
tashworth 8:77a57909aa15 785 /*************CHECK FOR WAVE OPENING*****************/
tashworth 8:77a57909aa15 786 /* If after 20 ms the ultrasonic still sees 20+ cm */
tashworth 8:77a57909aa15 787 /* then robot is at wave opening */
tashworth 8:77a57909aa15 788
tashworth 8:77a57909aa15 789 //pc.printf("wall follow 2 range %f\r\n",range);
tashworth 8:77a57909aa15 790 //pc.printf("loc+location = %f\r\n", loc+location);
tashworth 8:77a57909aa15 791 if(range > 20) {
tashworth 8:77a57909aa15 792 motors.stopBothMotors();
tashworth 8:77a57909aa15 793 pc.printf("wavegap\r\n");
tashworth 8:77a57909aa15 794 // AT WAVE OPENING!!!!
tashworth 8:77a57909aa15 795 break;
tashworth 8:77a57909aa15 796 }
tashworth 8:77a57909aa15 797
tashworth 8:77a57909aa15 798 pid1.setProcessValue(range);
tashworth 8:77a57909aa15 799 pid_return = pid1.compute();
tashworth 8:77a57909aa15 800 //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
tashworth 8:77a57909aa15 801
tashworth 8:77a57909aa15 802 if(pid_return > 0) {
tashworth 8:77a57909aa15 803 if(side) {
tashworth 8:77a57909aa15 804 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
tashworth 8:77a57909aa15 805 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 806 } else {
tashworth 8:77a57909aa15 807 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
tashworth 8:77a57909aa15 808 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 809 }
tashworth 8:77a57909aa15 810 } else if(pid_return < 0) {
tashworth 8:77a57909aa15 811 if(side) {
tashworth 8:77a57909aa15 812 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 813 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
tashworth 8:77a57909aa15 814 } else {
tashworth 8:77a57909aa15 815 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 816 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
tashworth 8:77a57909aa15 817 }
tashworth 8:77a57909aa15 818 } else {
tashworth 8:77a57909aa15 819 motors.setMotor0Speed(dir*MAX_SPEED);
tashworth 8:77a57909aa15 820 motors.setMotor1Speed(dir*MAX_SPEED);
tashworth 8:77a57909aa15 821 }
tashworth 8:77a57909aa15 822 }
tashworth 8:77a57909aa15 823 motors.stopBothMotors();
tashworth 8:77a57909aa15 824 }
tashworth 0:1b64a0cedc5d 825
tashworth 0:1b64a0cedc5d 826
tashworth 8:77a57909aa15 827 void alignWithWall(int section)
tashworth 8:77a57909aa15 828 {
tashworth 8:77a57909aa15 829 float usValue = 0;
tashworth 8:77a57909aa15 830
tashworth 8:77a57909aa15 831 if(section == TOOLS) {
tashworth 8:77a57909aa15 832 // turn at an angle
tashworth 8:77a57909aa15 833 leftEncoder.reset();
tashworth 8:77a57909aa15 834 rightEncoder.reset();
tashworth 8:77a57909aa15 835 motors.setMotor0Speed(-1.2*MAX_SPEED); //right
tashworth 8:77a57909aa15 836 motors.setMotor1Speed(0.4*MAX_SPEED); //left
tashworth 8:77a57909aa15 837 while(rightEncoder.getPulses()>-1000);
tashworth 8:77a57909aa15 838 motors.stopBothMotors();
tashworth 8:77a57909aa15 839
tashworth 8:77a57909aa15 840 //go backwards toward wall
tashworth 8:77a57909aa15 841 leftEncoder.reset();
tashworth 8:77a57909aa15 842 rightEncoder.reset();
tashworth 8:77a57909aa15 843 motors.setMotor0Speed(-MAX_SPEED); //right
tashworth 8:77a57909aa15 844 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 8:77a57909aa15 845 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
tashworth 8:77a57909aa15 846
tashworth 8:77a57909aa15 847 motors.stopBothMotors();
tashworth 8:77a57909aa15 848
tashworth 8:77a57909aa15 849 // turn left towards wall
tashworth 8:77a57909aa15 850 leftEncoder.reset();
tashworth 8:77a57909aa15 851 rightEncoder.reset();
tashworth 8:77a57909aa15 852 motors.setMotor0Speed(MAX_SPEED); //right
tashworth 8:77a57909aa15 853 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 8:77a57909aa15 854 while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
tashworth 8:77a57909aa15 855
tashworth 8:77a57909aa15 856 motors.stopBothMotors();
tashworth 8:77a57909aa15 857
tashworth 8:77a57909aa15 858 motors.setMotor0Speed(0.7*MAX_SPEED); //right
tashworth 8:77a57909aa15 859 motors.setMotor1Speed(-0.7*MAX_SPEED); //left
tashworth 8:77a57909aa15 860 } else {
tashworth 8:77a57909aa15 861 rightTurn();
tashworth 8:77a57909aa15 862 motors.setMotor0Speed(-0.7*MAX_SPEED); //right
tashworth 8:77a57909aa15 863 motors.setMotor1Speed(0.7*MAX_SPEED); //left
tashworth 8:77a57909aa15 864 }
tashworth 8:77a57909aa15 865
tashworth 8:77a57909aa15 866 usValue = 0;
tashworth 8:77a57909aa15 867 while(1) {
tashworth 8:77a57909aa15 868 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 869 wait_ms(20);
tashworth 8:77a57909aa15 870 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 871 //pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
tashworth 8:77a57909aa15 872 if(range > usValue && usValue != 0 && range < 25) {
tashworth 8:77a57909aa15 873 break;
tashworth 8:77a57909aa15 874 } else {
tashworth 8:77a57909aa15 875 usValue = range;
tashworth 8:77a57909aa15 876 }
tashworth 8:77a57909aa15 877 }
tashworth 8:77a57909aa15 878 motors.stopBothMotors();
tashworth 8:77a57909aa15 879 }
tashworth 8:77a57909aa15 880
tashworth 8:77a57909aa15 881 void rightTurn(void)
tashworth 8:77a57909aa15 882 {
tashworth 8:77a57909aa15 883 motors.begin();
tashworth 8:77a57909aa15 884 leftEncoder.reset();
tashworth 8:77a57909aa15 885 rightEncoder.reset();
tashworth 8:77a57909aa15 886 motors.setMotor0Speed(-0.5*127);//right
tashworth 8:77a57909aa15 887 motors.setMotor1Speed(0.5*127);//left
tashworth 8:77a57909aa15 888 while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
tashworth 8:77a57909aa15 889 motors.stopBothMotors();
tashworth 8:77a57909aa15 890 }
tashworth 8:77a57909aa15 891
tashworth 8:77a57909aa15 892 void leftTurn(void)
tashworth 8:77a57909aa15 893 {
tashworth 8:77a57909aa15 894 /*
tashworth 8:77a57909aa15 895 leftEncoder.reset();
tashworth 8:77a57909aa15 896 rightEncoder.reset();
tashworth 8:77a57909aa15 897 motors.setMotor0Speed(0.4*MAX_SPEED); //right
tashworth 8:77a57909aa15 898 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 8:77a57909aa15 899 while(abs(leftEncoder.getPulses())<2500);
tashworth 8:77a57909aa15 900 motors.stopBothMotors();
tashworth 8:77a57909aa15 901 */
tashworth 8:77a57909aa15 902 motors.begin();
tashworth 8:77a57909aa15 903 leftEncoder.reset();
tashworth 8:77a57909aa15 904 rightEncoder.reset();
tashworth 8:77a57909aa15 905 motors.setMotor0Speed(0.5*127);// right
tashworth 8:77a57909aa15 906 motors.setMotor1Speed(-0.5*127);// left
tashworth 8:77a57909aa15 907 while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
tashworth 8:77a57909aa15 908 motors.stopBothMotors();
tashworth 8:77a57909aa15 909 }
tashworth 8:77a57909aa15 910 void slightleft(void)
tashworth 8:77a57909aa15 911 {
tashworth 8:77a57909aa15 912
tashworth 8:77a57909aa15 913 leftEncoder.reset();
tashworth 8:77a57909aa15 914 rightEncoder.reset();
tashworth 8:77a57909aa15 915 motors.setMotor0Speed(0.5*127);// right
tashworth 8:77a57909aa15 916 motors.setMotor1Speed(-0.5*127);// left
tashworth 8:77a57909aa15 917 while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50);
tashworth 8:77a57909aa15 918 motors.stopBothMotors();
tashworth 8:77a57909aa15 919 }
tashworth 8:77a57909aa15 920
tashworth 8:77a57909aa15 921
tashworth 8:77a57909aa15 922 void overBump(int section)
tashworth 8:77a57909aa15 923 {
tashworth 8:77a57909aa15 924 int preLeft=5000, preRight=5000, out=0;
tashworth 8:77a57909aa15 925
tashworth 8:77a57909aa15 926 motors.begin();
tashworth 8:77a57909aa15 927
tashworth 8:77a57909aa15 928 leftEncoder.reset();
tashworth 8:77a57909aa15 929 rightEncoder.reset();
tashworth 8:77a57909aa15 930 motors.setMotor0Speed(-0.2*127); //right
tashworth 8:77a57909aa15 931 motors.setMotor1Speed(-0.2*127); //left
tashworth 8:77a57909aa15 932 while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
tashworth 8:77a57909aa15 933 motors.stopBothMotors();
tashworth 8:77a57909aa15 934
tashworth 8:77a57909aa15 935 leftEncoder.reset();
tashworth 8:77a57909aa15 936 rightEncoder.reset();
tashworth 8:77a57909aa15 937 motors.setMotor0Speed(0.2*127); //right
tashworth 8:77a57909aa15 938 motors.setMotor1Speed(0.2*127); //left
tashworth 9:1b29cd9ed1ba 939 while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0) {
tashworth 8:77a57909aa15 940 preLeft=leftEncoder.getPulses();
tashworth 8:77a57909aa15 941 preRight=rightEncoder.getPulses();
tashworth 8:77a57909aa15 942 wait_ms(100);
tashworth 8:77a57909aa15 943 //pc.printf(" first while left %d right %d \r\n", preLeft, preRight);
tashworth 8:77a57909aa15 944 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
tashworth 8:77a57909aa15 945 }
tashworth 8:77a57909aa15 946
tashworth 8:77a57909aa15 947 motors.stopBothMotors();
tashworth 8:77a57909aa15 948 motors.begin();
tashworth 8:77a57909aa15 949 wait(2);
tashworth 8:77a57909aa15 950 /*
tashworth 8:77a57909aa15 951 motors.stopBothMotors();
tashworth 8:77a57909aa15 952 motors.setMotor0Speed(0.15*127); //right
tashworth 8:77a57909aa15 953 motors.setMotor1Speed(0.15*127); //left
tashworth 8:77a57909aa15 954 preLeft=preRight=5000 ;
tashworth 8:77a57909aa15 955 leftEncoder.reset();
tashworth 8:77a57909aa15 956 rightEncoder.reset();
tashworth 8:77a57909aa15 957 */
tashworth 9:1b29cd9ed1ba 958 // while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0){
tashworth 8:77a57909aa15 959 /* preLeft=leftEncoder.getPulses();
tashworth 8:77a57909aa15 960 preRight=rightEncoder.getPulses();
tashworth 8:77a57909aa15 961 pc.printf("second while left %d right %d \r\n", preLeft, preRight);
tashworth 8:77a57909aa15 962 wait_ms(200);
tashworth 8:77a57909aa15 963 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
tashworth 8:77a57909aa15 964 }*/
tashworth 8:77a57909aa15 965
tashworth 8:77a57909aa15 966 leftEncoder.reset();
tashworth 8:77a57909aa15 967 rightEncoder.reset();
tashworth 8:77a57909aa15 968 motors.setMotor0Speed(0.3*127); //right
tashworth 8:77a57909aa15 969 motors.setMotor1Speed(0.3*127); //left
tashworth 8:77a57909aa15 970
tashworth 8:77a57909aa15 971 while(!out) {
tashworth 8:77a57909aa15 972 preLeft=leftEncoder.getPulses();
tashworth 8:77a57909aa15 973 preRight=rightEncoder.getPulses();
tashworth 8:77a57909aa15 974
tashworth 8:77a57909aa15 975 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 976 rangeFinderRight.startMeas();
tashworth 8:77a57909aa15 977 wait_ms(20);
tashworth 8:77a57909aa15 978 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 979 rangeFinderRight.getMeas(range2);
tashworth 8:77a57909aa15 980 if(range < 10 || range2 < 10) out=1;
tashworth 8:77a57909aa15 981
tashworth 8:77a57909aa15 982 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
tashworth 8:77a57909aa15 983 motors.setMotor0Speed(0.4*127); //right
tashworth 8:77a57909aa15 984 motors.setMotor1Speed(0.4*127); //left
tashworth 8:77a57909aa15 985 }
tashworth 8:77a57909aa15 986 if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1;
tashworth 8:77a57909aa15 987 }
tashworth 8:77a57909aa15 988
tashworth 8:77a57909aa15 989 motors.stopBothMotors();
tashworth 8:77a57909aa15 990 wait(2);
tashworth 8:77a57909aa15 991 motors.begin();
tashworth 8:77a57909aa15 992
tashworth 8:77a57909aa15 993 preLeft=preRight=5000 ;
tashworth 8:77a57909aa15 994 leftEncoder.reset();
tashworth 8:77a57909aa15 995 rightEncoder.reset();
tashworth 8:77a57909aa15 996 motors.setMotor0Speed(.25*127); //right
tashworth 8:77a57909aa15 997 motors.setMotor1Speed(.25*127); //left
tashworth 8:77a57909aa15 998
tashworth 8:77a57909aa15 999 if(section == TOOLS || section == MID) {
tashworth 9:1b29cd9ed1ba 1000 while(IR.getIRDistance() > 20 ) {
tashworth 9:1b29cd9ed1ba 1001 //pc.printf("IR %f\r\n", IR.getIRDistance());
tashworth 8:77a57909aa15 1002 //pc.printf("third while left %d right %d \r\n", preLeft, preRight);
tashworth 8:77a57909aa15 1003 }
tashworth 8:77a57909aa15 1004 } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
tashworth 8:77a57909aa15 1005
tashworth 8:77a57909aa15 1006 motors.setMotor0Speed(-.25*127); //right
tashworth 8:77a57909aa15 1007 motors.setMotor1Speed(-.25*127); //left
tashworth 8:77a57909aa15 1008 wait_ms(10);
tashworth 8:77a57909aa15 1009 motors.stopBothMotors();
tashworth 8:77a57909aa15 1010 wait(2);
tashworth 8:77a57909aa15 1011 motors.begin();
tashworth 8:77a57909aa15 1012
tashworth 8:77a57909aa15 1013 }
tashworth 8:77a57909aa15 1014
tashworth 8:77a57909aa15 1015 void to_tools_section(float* location, float &current)
tashworth 8:77a57909aa15 1016 {
tashworth 8:77a57909aa15 1017 wall_follow(LEFT,FORWARD, TOOLS);
tashworth 8:77a57909aa15 1018 // current position in reference to the starting position
tashworth 8:77a57909aa15 1019 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 1020 pc.printf("current %f \r\n",current);
tashworth 10:1a1d52207f59 1021
tashworth 8:77a57909aa15 1022 motors.stopBothMotors();
tashworth 8:77a57909aa15 1023
tashworth 9:1b29cd9ed1ba 1024 wait(2);
tashworth 9:1b29cd9ed1ba 1025
tashworth 9:1b29cd9ed1ba 1026 motors.setMotor0Speed(-.2*127); //right
tashworth 9:1b29cd9ed1ba 1027 motors.setMotor1Speed(-.2*127); //left
tashworth 9:1b29cd9ed1ba 1028 wait_ms(5);
tashworth 9:1b29cd9ed1ba 1029 motors.stopBothMotors();
tashworth 8:77a57909aa15 1030
tashworth 8:77a57909aa15 1031 }
tashworth 8:77a57909aa15 1032
tashworth 8:77a57909aa15 1033 void from_tools_section(float* location, float &current)
tashworth 8:77a57909aa15 1034 {
tashworth 8:77a57909aa15 1035 alignWithWall(TOOLS);
tashworth 8:77a57909aa15 1036
tashworth 8:77a57909aa15 1037 wait_ms(100);
tashworth 8:77a57909aa15 1038
tashworth 8:77a57909aa15 1039 wall_follow2(LEFT,FORWARD,MID, current);
tashworth 8:77a57909aa15 1040 current= 78;
tashworth 8:77a57909aa15 1041
tashworth 8:77a57909aa15 1042 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 1043 wait_ms(20);
tashworth 8:77a57909aa15 1044 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 1045
tashworth 8:77a57909aa15 1046 if(range < 20) {
tashworth 8:77a57909aa15 1047 wall_follow2(LEFT,BACKWARD,TOOLS, current);
tashworth 8:77a57909aa15 1048 leftEncoder.reset();
tashworth 8:77a57909aa15 1049 rightEncoder.reset();
tashworth 8:77a57909aa15 1050 motors.setMotor0Speed(-MAX_SPEED); //right
tashworth 8:77a57909aa15 1051 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 8:77a57909aa15 1052 while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
tashworth 8:77a57909aa15 1053
tashworth 8:77a57909aa15 1054 leftEncoder.reset();
tashworth 8:77a57909aa15 1055 rightEncoder.reset();
tashworth 8:77a57909aa15 1056 motors.setMotor0Speed(MAX_SPEED); //right
tashworth 8:77a57909aa15 1057 motors.setMotor1Speed(MAX_SPEED); //left
tashworth 8:77a57909aa15 1058 while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
tashworth 8:77a57909aa15 1059 motors.stopBothMotors();
tashworth 8:77a57909aa15 1060
tashworth 8:77a57909aa15 1061 wait_ms(500);
tashworth 8:77a57909aa15 1062 location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1063 current= location[0];
tashworth 8:77a57909aa15 1064 leftTurn();
tashworth 8:77a57909aa15 1065 slightleft();
tashworth 8:77a57909aa15 1066 overBump(TOOLS);
tashworth 8:77a57909aa15 1067 } else {
tashworth 8:77a57909aa15 1068 location[0]= 77;
tashworth 8:77a57909aa15 1069 leftTurn();
tashworth 8:77a57909aa15 1070 wait_ms(20);
tashworth 8:77a57909aa15 1071 overBump(FIRST_WAVE);
tashworth 8:77a57909aa15 1072 }
tashworth 8:77a57909aa15 1073
tashworth 8:77a57909aa15 1074 pc.printf("First Wavegap = %f\r\n",location[0]);
tashworth 8:77a57909aa15 1075 }
tashworth 8:77a57909aa15 1076 void mid_section(float* location, float &current, int* direction)
tashworth 8:77a57909aa15 1077 {
tashworth 8:77a57909aa15 1078
tashworth 8:77a57909aa15 1079 motors.begin();
tashworth 8:77a57909aa15 1080
tashworth 9:1b29cd9ed1ba 1081 if(IR.getIRDistance() > 20) return;
tashworth 8:77a57909aa15 1082
tashworth 8:77a57909aa15 1083 alignWithWall(MID);
tashworth 8:77a57909aa15 1084
tashworth 8:77a57909aa15 1085 pc.printf("mid section current = %f\r\n",current);
tashworth 8:77a57909aa15 1086 wall_follow2(LEFT,FORWARD,MID, current);
tashworth 8:77a57909aa15 1087 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1088 pc.printf("after wf2 current = %f\r\n",current);
tashworth 8:77a57909aa15 1089
tashworth 8:77a57909aa15 1090 if(current != 0) {
tashworth 8:77a57909aa15 1091 direction[0]= RIGHT;
tashworth 8:77a57909aa15 1092 current+= location[0];
tashworth 8:77a57909aa15 1093 location[1]= current;
tashworth 8:77a57909aa15 1094 } else {
tashworth 8:77a57909aa15 1095 current=location[0];
tashworth 8:77a57909aa15 1096 direction[0]= LEFT;
tashworth 8:77a57909aa15 1097 wall_follow2(LEFT,BACKWARD,MID,current);
tashworth 8:77a57909aa15 1098 location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1099 }
tashworth 8:77a57909aa15 1100
tashworth 8:77a57909aa15 1101 pc.printf("wavegap2 = %f\r\n",location[1]);
tashworth 8:77a57909aa15 1102 leftTurn();
tashworth 8:77a57909aa15 1103 overBump(TOOLS);
tashworth 8:77a57909aa15 1104 // go forward
tashworth 8:77a57909aa15 1105 leftEncoder.reset();
tashworth 8:77a57909aa15 1106 rightEncoder.reset();
tashworth 8:77a57909aa15 1107 motors.setMotor0Speed(0.2*127); //right
tashworth 8:77a57909aa15 1108 motors.setMotor1Speed(0.2*127); //left
tashworth 8:77a57909aa15 1109 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
tashworth 8:77a57909aa15 1110 motors.stopBothMotors();
tashworth 8:77a57909aa15 1111
tashworth 8:77a57909aa15 1112 }
tashworth 8:77a57909aa15 1113
tashworth 8:77a57909aa15 1114 void mid_section2(float* location, float &current, int* direction)
tashworth 8:77a57909aa15 1115 {
tashworth 8:77a57909aa15 1116
tashworth 8:77a57909aa15 1117 motors.begin();
tashworth 8:77a57909aa15 1118
tashworth 9:1b29cd9ed1ba 1119 if(IR.getIRDistance() > 20) return;
tashworth 8:77a57909aa15 1120
tashworth 8:77a57909aa15 1121 alignWithWall(MID);
tashworth 8:77a57909aa15 1122 wall_follow2(LEFT,FORWARD,MID, current);
tashworth 8:77a57909aa15 1123 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1124
tashworth 8:77a57909aa15 1125 if(current != 0) {
tashworth 8:77a57909aa15 1126 direction[1]= RIGHT;
tashworth 8:77a57909aa15 1127 current+= location[1];
tashworth 8:77a57909aa15 1128 location[2]= current;
tashworth 8:77a57909aa15 1129 } else {
tashworth 8:77a57909aa15 1130 current=location[1];
tashworth 8:77a57909aa15 1131 direction[1]= LEFT;
tashworth 8:77a57909aa15 1132 wall_follow2(LEFT,BACKWARD,MID,current);
tashworth 8:77a57909aa15 1133 location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1134 }
tashworth 8:77a57909aa15 1135
tashworth 8:77a57909aa15 1136 leftTurn();
tashworth 8:77a57909aa15 1137 overBump(RIGS);
tashworth 8:77a57909aa15 1138 }
tashworth 8:77a57909aa15 1139
tashworth 8:77a57909aa15 1140 void rig_section(float* location, float &current, int* direction, int rig)
tashworth 8:77a57909aa15 1141 {
tashworth 8:77a57909aa15 1142
tashworth 8:77a57909aa15 1143
tashworth 8:77a57909aa15 1144 }
tashworth 8:77a57909aa15 1145
tashworth 9:1b29cd9ed1ba 1146 void ledtoggle(void)
tashworth 9:1b29cd9ed1ba 1147 {
tashworth 8:77a57909aa15 1148 pwm.setPWM(9, 0, 4094);
tashworth 8:77a57909aa15 1149 wait(2);
tashworth 8:77a57909aa15 1150 pwm.setPWM(9, 0, 0);
tashworth 9:1b29cd9ed1ba 1151 }
tashworth 8:77a57909aa15 1152
tashworth 8:77a57909aa15 1153
tashworth 8:77a57909aa15 1154