Thomas Ashworth / theRobot

For IEEE Robotics

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

main.cpp@9:1b29cd9ed1ba, 2014-03-28 (annotated)

Committer:
tashworth
Date:
Fri Mar 28 15:32:16 2014 +0000
Revision:
9:1b29cd9ed1ba
Parent:
8:77a57909aa15
Child:
10:1a1d52207f59
3/28/14 10:32AM

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 9:1b29cd9ed1ba 183 {OIL_RIG1, 164, 90, 90, 52, 100, 0}, // point laser at oilrig1
tashworth 9:1b29cd9ed1ba 184 {OIL_RIG2, 145, 90, 90, 51, 100, 0}, // point laser at oilrig2
tashworth 8:77a57909aa15 185 {OIL_RIG3, 130, 90, 90, 50, 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 8:77a57909aa15 213 float location[3], current=0;
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 6:75259c3306dd 226 //ServoOutputDisable = 0;
tashworth 6:75259c3306dd 227
tashworth 6:75259c3306dd 228 /*******************
tashworth 6:75259c3306dd 229 WHILE LOOP FOR TESTING
tashworth 6:75259c3306dd 230 ********************/
tashworth 9:1b29cd9ed1ba 231 /*while(1) {
tashworth 8:77a57909aa15 232 pc.scanf("%d %d", &servoNum, &servoAngle);
tashworth 8:77a57909aa15 233 if(servoAngle > 175) {
tashworth 8:77a57909aa15 234 servoAngle = 175;
tashworth 8:77a57909aa15 235 }
tashworth 8:77a57909aa15 236 if(servoNum > 5 ) {
tashworth 8:77a57909aa15 237 servoNum = 0;
tashworth 8:77a57909aa15 238 servoAngle = 90;
tashworth 8:77a57909aa15 239 }
tashworth 8:77a57909aa15 240 setServoPulse(servoNum, servoAngle);
tashworth 6:75259c3306dd 241
tashworth 8:77a57909aa15 242 //shape_detected = shapeDetection();
tashworth 8:77a57909aa15 243 //distLaser = getDistance();
tashworth 8:77a57909aa15 244 //pc.printf("Distance %d", distLaser);
tashworth 9:1b29cd9ed1ba 245 //ledtoggle();
tashworth 8:77a57909aa15 246
tashworth 9:1b29cd9ed1ba 247 pc.scanf("%d", &posNum);
tashworth 9:1b29cd9ed1ba 248 servoPosition(posNum);
tashworth 9:1b29cd9ed1ba 249 wait(5);
tashworth 9:1b29cd9ed1ba 250 //shape_detected = shapeDetection();
tashworth 9:1b29cd9ed1ba 251 distLaser = getDistance();
tashworth 9:1b29cd9ed1ba 252 pc.printf("Distance %d", distLaser);
tashworth 9:1b29cd9ed1ba 253
tashworth 9:1b29cd9ed1ba 254 }*/
tashworth 3:b7b4780a7f6e 255
tashworth 7:8fb4204f9600 256 /********************************
tashworth 7:8fb4204f9600 257 MAIN WHILE LOOP FOR COMPETITION
tashworth 7:8fb4204f9600 258 *********************************/
tashworth 3:b7b4780a7f6e 259 while(1) {
tashworth 9:1b29cd9ed1ba 260
tashworth 6:75259c3306dd 261 switch (state) {
tashworth 8:77a57909aa15 262
tashworth 7:8fb4204f9600 263 /**************************************************
tashworth 7:8fb4204f9600 264 * STAGE 0
tashworth 7:8fb4204f9600 265 *
tashworth 7:8fb4204f9600 266 * - START OF THE COMETITION
tashworth 7:8fb4204f9600 267 *
tashworth 7:8fb4204f9600 268 **************************************************/
tashworth 6:75259c3306dd 269 case START :
tashworth 6:75259c3306dd 270 myled1 = 1;
tashworth 6:75259c3306dd 271 wait(5);
tashworth 6:75259c3306dd 272 myled1 = 0;
tashworth 6:75259c3306dd 273 state = OILRIG1_POS;
tashworth 3:b7b4780a7f6e 274 break;
tashworth 3:b7b4780a7f6e 275
tashworth 3:b7b4780a7f6e 276
tashworth 6:75259c3306dd 277 /**************************************************
tashworth 6:75259c3306dd 278 * STAGE 1
tashworth 6:75259c3306dd 279 *
tashworth 6:75259c3306dd 280 * - DETERMINE OIL RIG ON FIRE
tashworth 6:75259c3306dd 281 *
tashworth 6:75259c3306dd 282 **************************************************/
tashworth 6:75259c3306dd 283 case OILRIG1_POS: //aims arm at square oil rig
tashworth 6:75259c3306dd 284 servoPosition(OIL_RIG1); //position arm to point at first oilrig
tashworth 9:1b29cd9ed1ba 285 wait(5); //wait for servos to settle
tashworth 6:75259c3306dd 286 fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
tashworth 6:75259c3306dd 287
tashworth 6:75259c3306dd 288 //determines what tool is needed
tashworth 6:75259c3306dd 289 if (fire == 1) {
tashworth 6:75259c3306dd 290 tool_needed = SQUARE;
tashworth 6:75259c3306dd 291 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 292 } else {
tashworth 6:75259c3306dd 293 state = OILRIG2_POS;
tashworth 6:75259c3306dd 294 }
tashworth 9:1b29cd9ed1ba 295
tashworth 6:75259c3306dd 296 break;
tashworth 6:75259c3306dd 297
tashworth 6:75259c3306dd 298 case OILRIG2_POS:
tashworth 6:75259c3306dd 299 servoPosition(OIL_RIG2); //position arm to point at first oilrig
tashworth 6:75259c3306dd 300 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 301 fire = fire_checker(OIL_RIG2);
tashworth 6:75259c3306dd 302 if (fire == 1) {
tashworth 6:75259c3306dd 303 tool_needed = TRIANGLE;
tashworth 6:75259c3306dd 304 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 305 } else {
tashworth 6:75259c3306dd 306 tool_needed = CIRCLE;
tashworth 6:75259c3306dd 307 state = GOTO_TOOLS;
tashworth 6:75259c3306dd 308 }
tashworth 9:1b29cd9ed1ba 309 pc.printf("tool needed: %d", tool_needed);
tashworth 6:75259c3306dd 310 break;
tashworth 6:75259c3306dd 311
tashworth 6:75259c3306dd 312 /**************************************************
tashworth 6:75259c3306dd 313 * STAGE 2
tashworth 6:75259c3306dd 314 *
tashworth 6:75259c3306dd 315 * - TRAVEL TO TOOLS
tashworth 6:75259c3306dd 316 *
tashworth 6:75259c3306dd 317 **************************************************/
tashworth 6:75259c3306dd 318 case GOTO_TOOLS:
tashworth 6:75259c3306dd 319 servoPosition(DRIVE_POSITION_NOTOOL); //drive position without a tool
tashworth 9:1b29cd9ed1ba 320 wait(2); //wait for servos to settle
tashworth 8:77a57909aa15 321
tashworth 8:77a57909aa15 322 to_tools_section(location, current);
tashworth 6:75259c3306dd 323
tashworth 6:75259c3306dd 324 state = IDENTIFY_TOOLS;
tashworth 9:1b29cd9ed1ba 325 pc.printf("YES!!!!!");
tashworth 9:1b29cd9ed1ba 326 while(1);
tashworth 6:75259c3306dd 327 break;
tashworth 6:75259c3306dd 328 /**************************************************
tashworth 6:75259c3306dd 329 * STAGE 3
tashworth 6:75259c3306dd 330 *
tashworth 6:75259c3306dd 331 * - Determine order of tools
tashworth 6:75259c3306dd 332 * - Aquire appropriate tool
tashworth 6:75259c3306dd 333 *
tashworth 6:75259c3306dd 334 **************************************************/
tashworth 6:75259c3306dd 335 case IDENTIFY_TOOLS:
tashworth 6:75259c3306dd 336 servoPosition(TOOL_1); //arm/camera looks over tool
tashworth 6:75259c3306dd 337 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 338 centerCamWithTool(); //centers camera over tool
tashworth 6:75259c3306dd 339 shape_detected = shapeDetection(); //determines the shape
tashworth 3:b7b4780a7f6e 340
tashworth 6:75259c3306dd 341 //either goes to aquire the tool or look at the next shape
tashworth 6:75259c3306dd 342 if(shape_detected == tool_needed) {
tashworth 6:75259c3306dd 343 state = AQUIRE_TOOL1;
tashworth 6:75259c3306dd 344 } else {
tashworth 6:75259c3306dd 345 servoPosition(TOOL_2);
tashworth 6:75259c3306dd 346 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 347 centerCamWithTool();
tashworth 6:75259c3306dd 348 shape_detected = shapeDetection();
tashworth 6:75259c3306dd 349 if (shape_detected == tool_needed) {
tashworth 6:75259c3306dd 350 state = AQUIRE_TOOL2;
tashworth 6:75259c3306dd 351 } else {
tashworth 6:75259c3306dd 352 servoPosition(TOOL_3);
tashworth 6:75259c3306dd 353 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 354 centerCamWithTool();
tashworth 6:75259c3306dd 355 state = AQUIRE_TOOL3;
tashworth 6:75259c3306dd 356 }
tashworth 6:75259c3306dd 357 }
tashworth 6:75259c3306dd 358 break;
tashworth 6:75259c3306dd 359 case AQUIRE_TOOL1:
tashworth 6:75259c3306dd 360 //*********************//
tashworth 6:75259c3306dd 361 //******* TODO ********//
tashworth 6:75259c3306dd 362 //*********************//
tashworth 6:75259c3306dd 363 // CODE TO GRAB TOOL1
tashworth 6:75259c3306dd 364 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 365 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 366 break;
tashworth 6:75259c3306dd 367 case AQUIRE_TOOL2:
tashworth 6:75259c3306dd 368 //*********************//
tashworth 6:75259c3306dd 369 //******* TODO ********//
tashworth 6:75259c3306dd 370 //*********************//
tashworth 6:75259c3306dd 371 // CODE TO GRAB TOOL2
tashworth 6:75259c3306dd 372 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 373 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 374 break;
tashworth 6:75259c3306dd 375 case AQUIRE_TOOL3:
tashworth 6:75259c3306dd 376 //*********************//
tashworth 6:75259c3306dd 377 //******* TODO ********//
tashworth 6:75259c3306dd 378 //*********************//
tashworth 6:75259c3306dd 379 // CODE TO GRAB TOOL3
tashworth 6:75259c3306dd 380 servoPosition(DRIVE_POSITION_TOOL); //arm position for driving with the tool
tashworth 6:75259c3306dd 381 state = NAVIGATE_WAVES_ROW1;
tashworth 6:75259c3306dd 382 break;
tashworth 3:b7b4780a7f6e 383
tashworth 0:1b64a0cedc5d 384
tashworth 6:75259c3306dd 385 /**************************************************
tashworth 6:75259c3306dd 386 * STAGE 4
tashworth 6:75259c3306dd 387 *
tashworth 6:75259c3306dd 388 * - Navigate through the ocean
tashworth 6:75259c3306dd 389 *
tashworth 6:75259c3306dd 390 **************************************************/
tashworth 6:75259c3306dd 391
tashworth 6:75259c3306dd 392 case NAVIGATE_WAVES_ROW1:
tashworth 6:75259c3306dd 393 //*********************//
tashworth 6:75259c3306dd 394 //******* TODO ********//
tashworth 6:75259c3306dd 395 //*********************//
tashworth 6:75259c3306dd 396 // CODE TO NAVIGATE ROW1
tashworth 6:75259c3306dd 397 state = NAVIGATE_WAVES_ROW2;
tashworth 6:75259c3306dd 398 break;
tashworth 6:75259c3306dd 399
tashworth 6:75259c3306dd 400 case NAVIGATE_WAVES_ROW2:
tashworth 6:75259c3306dd 401 //*********************//
tashworth 6:75259c3306dd 402 //******* TODO ********//
tashworth 6:75259c3306dd 403 //*********************//
tashworth 6:75259c3306dd 404 // CODE TO NAVIGATE ROW2
tashworth 6:75259c3306dd 405 state = NAVIGATE_WAVES_ROW3;
tashworth 6:75259c3306dd 406 break;
tashworth 3:b7b4780a7f6e 407
tashworth 6:75259c3306dd 408 case NAVIGATE_WAVES_ROW3:
tashworth 6:75259c3306dd 409 //*********************//
tashworth 6:75259c3306dd 410 //******* TODO ********//
tashworth 6:75259c3306dd 411 //*********************//
tashworth 6:75259c3306dd 412 // CODE TO NAVIGATE ROW3
tashworth 7:8fb4204f9600 413
tashworth 6:75259c3306dd 414 //goes to appropriate rig
tashworth 6:75259c3306dd 415 if(shape_detected == 1) {
tashworth 6:75259c3306dd 416 state = NAVIGATE_TO_SQUARE_RIG;
tashworth 6:75259c3306dd 417 } else if(shape_detected == 2) {
tashworth 6:75259c3306dd 418 state = NAVIGATE_TO_TRIANGLE_RIG;
tashworth 6:75259c3306dd 419 } else {
tashworth 6:75259c3306dd 420 state = NAVIGATE_TO_CIRCLE_RIG;
tashworth 6:75259c3306dd 421 }
tashworth 6:75259c3306dd 422 break;
tashworth 6:75259c3306dd 423
tashworth 6:75259c3306dd 424 /**************************************************
tashworth 6:75259c3306dd 425 * STAGE 5
tashworth 6:75259c3306dd 426 *
tashworth 6:75259c3306dd 427 * - Travel to appropriate rig
tashworth 6:75259c3306dd 428 *
tashworth 6:75259c3306dd 429 **************************************************/
tashworth 6:75259c3306dd 430 case NAVIGATE_TO_SQUARE_RIG:
tashworth 6:75259c3306dd 431 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 432 state = RIG_ALIGN;
tashworth 6:75259c3306dd 433 break;
tashworth 6:75259c3306dd 434 case NAVIGATE_TO_TRIANGLE_RIG:
tashworth 6:75259c3306dd 435 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 436 state = RIG_ALIGN;
tashworth 6:75259c3306dd 437 break;
tashworth 6:75259c3306dd 438 case NAVIGATE_TO_CIRCLE_RIG:
tashworth 6:75259c3306dd 439 //NAVIGATION CODE HERE
tashworth 6:75259c3306dd 440 state = RIG_ALIGN;
tashworth 6:75259c3306dd 441 break;
tashworth 3:b7b4780a7f6e 442
tashworth 6:75259c3306dd 443 /**************************************************
tashworth 6:75259c3306dd 444 * STAGE 6
tashworth 6:75259c3306dd 445 *
tashworth 6:75259c3306dd 446 * - Align with appropriate rig
tashworth 6:75259c3306dd 447 *
tashworth 6:75259c3306dd 448 **************************************************/
tashworth 6:75259c3306dd 449 case RIG_ALIGN:
tashworth 7:8fb4204f9600 450
tashworth 6:75259c3306dd 451 //*********************//
tashworth 6:75259c3306dd 452 //******* TODO ********//
tashworth 6:75259c3306dd 453 //*********************//
tashworth 6:75259c3306dd 454 // CODE TO ALIGN ROBOT WITH RIG
tashworth 7:8fb4204f9600 455
tashworth 6:75259c3306dd 456 servoPosition(ORIENT_TOOL);
tashworth 6:75259c3306dd 457 wait(1); //wait for servos to settle
tashworth 6:75259c3306dd 458 state = INSERT_TOOL;
tashworth 6:75259c3306dd 459 break;
tashworth 3:b7b4780a7f6e 460
tashworth 6:75259c3306dd 461 /**************************************************
tashworth 6:75259c3306dd 462 * STAGE 7
tashworth 6:75259c3306dd 463 *
tashworth 6:75259c3306dd 464 * - Insert Tool
tashworth 6:75259c3306dd 465 * - Extenguish fire
tashworth 6:75259c3306dd 466 * - win contest
tashworth 6:75259c3306dd 467 *
tashworth 6:75259c3306dd 468 **************************************************/
tashworth 7:8fb4204f9600 469
tashworth 6:75259c3306dd 470 case INSERT_TOOL:
tashworth 6:75259c3306dd 471 //*********************//
tashworth 6:75259c3306dd 472 //******* TODO ********//
tashworth 6:75259c3306dd 473 //*********************//
tashworth 6:75259c3306dd 474 // CODE TO INSERT TOOL
tashworth 6:75259c3306dd 475 break;
tashworth 3:b7b4780a7f6e 476
tashworth 6:75259c3306dd 477 /**************************************************
tashworth 6:75259c3306dd 478 * STAGE 8
tashworth 6:75259c3306dd 479 *
tashworth 6:75259c3306dd 480 * - END COMPETITION
tashworth 6:75259c3306dd 481 *
tashworth 6:75259c3306dd 482 **************************************************/
tashworth 6:75259c3306dd 483 case END:
tashworth 6:75259c3306dd 484 servoPosition(STORE_POSITION);
tashworth 6:75259c3306dd 485 myled1 = 1;
tashworth 6:75259c3306dd 486 wait(.2);
tashworth 6:75259c3306dd 487 myled2 = 1;
tashworth 6:75259c3306dd 488 wait(.2);
tashworth 6:75259c3306dd 489 myled3 = 1;
tashworth 6:75259c3306dd 490 wait(.2);
tashworth 6:75259c3306dd 491 myled4 = 1;
tashworth 6:75259c3306dd 492 wait(.2);
tashworth 6:75259c3306dd 493 break;
tashworth 6:75259c3306dd 494 default:
tashworth 3:b7b4780a7f6e 495
tashworth 6:75259c3306dd 496 break;
tashworth 6:75259c3306dd 497 }
tashworth 6:75259c3306dd 498 }
tashworth 6:75259c3306dd 499
tashworth 0:1b64a0cedc5d 500
tashworth 0:1b64a0cedc5d 501 }
tashworth 0:1b64a0cedc5d 502
tashworth 0:1b64a0cedc5d 503
tashworth 0:1b64a0cedc5d 504
tashworth 0:1b64a0cedc5d 505 /************
tashworth 0:1b64a0cedc5d 506
tashworth 0:1b64a0cedc5d 507 Servo Functions
tashworth 0:1b64a0cedc5d 508
tashworth 0:1b64a0cedc5d 509 **************/
tashworth 0:1b64a0cedc5d 510
tashworth 3:b7b4780a7f6e 511 void setServoPulse(uint8_t n, int angle)
tashworth 3:b7b4780a7f6e 512 {
tashworth 3:b7b4780a7f6e 513 int pulse;
tashworth 3:b7b4780a7f6e 514 pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 1:fe4a0b47ff25 515 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 516 int i = currentPosition[n];
tashworth 3:b7b4780a7f6e 517 pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
tashworth 3:b7b4780a7f6e 518 int pulse2;
tashworth 3:b7b4780a7f6e 519 if(currentPosition[n] < pulse) {
tashworth 3:b7b4780a7f6e 520 for(i; i < pulse; i++) {
tashworth 3:b7b4780a7f6e 521 pulse2 = 4094 * i / pulselength;
tashworth 3:b7b4780a7f6e 522 pwm.setPWM(n, 0, pulse2);
tashworth 3:b7b4780a7f6e 523 wait_ms(3);
tashworth 3:b7b4780a7f6e 524 }
tashworth 1:fe4a0b47ff25 525 } else if (currentPosition[n] > pulse) {
tashworth 3:b7b4780a7f6e 526 for(i; i > pulse; i--) {
tashworth 3:b7b4780a7f6e 527 pulse2 = 4094 * i / pulselength;
tashworth 3:b7b4780a7f6e 528 pwm.setPWM(n, 0, pulse2);
tashworth 3:b7b4780a7f6e 529 wait_ms(3);
tashworth 3:b7b4780a7f6e 530 }
tashworth 1:fe4a0b47ff25 531 }
tashworth 1:fe4a0b47ff25 532 currentPosition[n] = i;
tashworth 3:b7b4780a7f6e 533 pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle);
tashworth 0:1b64a0cedc5d 534 }
tashworth 0:1b64a0cedc5d 535
tashworth 3:b7b4780a7f6e 536 void initServoDriver(void)
tashworth 3:b7b4780a7f6e 537 {
tashworth 0:1b64a0cedc5d 538 pwm.begin();
tashworth 0:1b64a0cedc5d 539 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 0:1b64a0cedc5d 540 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 0:1b64a0cedc5d 541 pwm.setI2Cfreq(400000); //400kHz
tashworth 3:b7b4780a7f6e 542
tashworth 3:b7b4780a7f6e 543 }
tashworth 3:b7b4780a7f6e 544
tashworth 3:b7b4780a7f6e 545 void servoBegin(void)
tashworth 3:b7b4780a7f6e 546 {
tashworth 3:b7b4780a7f6e 547 pc.printf("Setting Initial Position\n\r");
tashworth 8:77a57909aa15 548 setServoPulseNo_delay(3, 175);
tashworth 8:77a57909aa15 549 wait(2);
tashworth 8:77a57909aa15 550 setServoPulseNo_delay(2, 0);
tashworth 8:77a57909aa15 551 wait(2);
tashworth 8:77a57909aa15 552 setServoPulseNo_delay(1, 10);
tashworth 8:77a57909aa15 553 wait(2);
tashworth 8:77a57909aa15 554 setServoPulseNo_delay(0, 85);
tashworth 8:77a57909aa15 555 wait(1);
tashworth 8:77a57909aa15 556 setServoPulseNo_delay(4, 100);
tashworth 8:77a57909aa15 557 wait(1);
tashworth 8:77a57909aa15 558 setServoPulseNo_delay(5, 0);
tashworth 3:b7b4780a7f6e 559 setGripper(1);
tashworth 0:1b64a0cedc5d 560 }
tashworth 0:1b64a0cedc5d 561
tashworth 3:b7b4780a7f6e 562 void setServoPulseNo_delay(uint8_t n, int angle)
tashworth 3:b7b4780a7f6e 563 {
tashworth 3:b7b4780a7f6e 564 int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 1:fe4a0b47ff25 565 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 566 currentPosition[n] = pulse;
tashworth 8:77a57909aa15 567 //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
tashworth 1:fe4a0b47ff25 568 pulse = 4094 * pulse / pulselength;
tashworth 1:fe4a0b47ff25 569 pwm.setPWM(n, 0, pulse);
tashworth 3:b7b4780a7f6e 570
tashworth 1:fe4a0b47ff25 571 }
tashworth 3:b7b4780a7f6e 572
tashworth 3:b7b4780a7f6e 573 void setGripper(int open)
tashworth 3:b7b4780a7f6e 574 {
tashworth 3:b7b4780a7f6e 575 if (open) {
tashworth 3:b7b4780a7f6e 576 pc.printf("Gripper Open\r");
tashworth 3:b7b4780a7f6e 577 setServoPulseNo_delay(6, 10);
tashworth 3:b7b4780a7f6e 578 } else {
tashworth 3:b7b4780a7f6e 579 pc.printf("Gripper Closed\n\r");
tashworth 3:b7b4780a7f6e 580 setServoPulseNo_delay(6, 170);
tashworth 3:b7b4780a7f6e 581 }
tashworth 3:b7b4780a7f6e 582 }
tashworth 3:b7b4780a7f6e 583
tashworth 3:b7b4780a7f6e 584 void servoPosition(int set)
tashworth 3:b7b4780a7f6e 585 {
tashworth 3:b7b4780a7f6e 586 //moves to current position
tashworth 8:77a57909aa15 587 setServoPulse(3, Arm_Table[set].claw_arm);
tashworth 3:b7b4780a7f6e 588 setServoPulse(1, Arm_Table[set].base_arm);
tashworth 6:75259c3306dd 589 setServoPulse(2, Arm_Table[set].big_arm);
tashworth 8:77a57909aa15 590 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 6:75259c3306dd 591 setServoPulse(4, Arm_Table[set].claw_rotate);
tashworth 6:75259c3306dd 592 setServoPulse(5, Arm_Table[set].claw_open);
tashworth 6:75259c3306dd 593 }
tashworth 6:75259c3306dd 594
tashworth 8:77a57909aa15 595
tashworth 6:75259c3306dd 596 int fire_checker(int rig)
tashworth 6:75259c3306dd 597 {
tashworth 6:75259c3306dd 598 switch (rig) {
tashworth 6:75259c3306dd 599
tashworth 6:75259c3306dd 600 case 1:
tashworth 6:75259c3306dd 601 for (int i = 0; i<5; i++) {
tashworth 8:77a57909aa15 602 distLaser = getDistance();
tashworth 9:1b29cd9ed1ba 603 pc.printf("L DISTANCE: %d", distLaser);
tashworth 8:77a57909aa15 604 if ((distLaser < OILRIG1_MAX)
tashworth 9:1b29cd9ed1ba 605 && (distLaser > OILRIG1_MIN)) {
tashworth 6:75259c3306dd 606 fire_detected++;
tashworth 6:75259c3306dd 607 } else {
tashworth 6:75259c3306dd 608 fire_not_detected++;
tashworth 6:75259c3306dd 609 }
tashworth 6:75259c3306dd 610 }
tashworth 9:1b29cd9ed1ba 611 break;
tashworth 6:75259c3306dd 612 case 2:
tashworth 6:75259c3306dd 613 for (int i = 0; i<5; i++) {
tashworth 8:77a57909aa15 614 distLaser = getDistance();
tashworth 9:1b29cd9ed1ba 615 pc.printf("L DISTANCE: %d", distLaser);
tashworth 8:77a57909aa15 616 if ((distLaser < OILRIG2_MAX)
tashworth 9:1b29cd9ed1ba 617 && (distLaser > OILRIG2_MIN)) {
tashworth 6:75259c3306dd 618 fire_detected++;
tashworth 6:75259c3306dd 619 } else {
tashworth 6:75259c3306dd 620 fire_not_detected++;
tashworth 6:75259c3306dd 621 }
tashworth 6:75259c3306dd 622 }
tashworth 9:1b29cd9ed1ba 623 break;
tashworth 6:75259c3306dd 624 case 3:
tashworth 6:75259c3306dd 625 for (int i = 0; i<5; i++) {
tashworth 8:77a57909aa15 626 distLaser = getDistance();
tashworth 8:77a57909aa15 627 if ((distLaser < OILRIG3_MAX)
tashworth 9:1b29cd9ed1ba 628 && (distLaser > OILRIG3_MIN)) {
tashworth 6:75259c3306dd 629 fire_detected++;
tashworth 6:75259c3306dd 630 } else {
tashworth 6:75259c3306dd 631 fire_not_detected++;
tashworth 6:75259c3306dd 632 }
tashworth 6:75259c3306dd 633 }
tashworth 9:1b29cd9ed1ba 634 break;
tashworth 9:1b29cd9ed1ba 635
tashworth 6:75259c3306dd 636 default:
tashworth 6:75259c3306dd 637 for (int i = 0; i<5; i++) {
tashworth 8:77a57909aa15 638 distLaser = getDistance();
tashworth 8:77a57909aa15 639 if ((distLaser < OILRIG1_MAX)
tashworth 8:77a57909aa15 640 || (distLaser > OILRIG1_MIN)) {
tashworth 6:75259c3306dd 641 fire_detected++;
tashworth 6:75259c3306dd 642 } else {
tashworth 6:75259c3306dd 643 fire_not_detected++;
tashworth 6:75259c3306dd 644 }
tashworth 6:75259c3306dd 645 }
tashworth 9:1b29cd9ed1ba 646 break;
tashworth 6:75259c3306dd 647 }
tashworth 6:75259c3306dd 648
tashworth 6:75259c3306dd 649 if (fire_detected > fire_not_detected) {
tashworth 6:75259c3306dd 650 return 1;
tashworth 6:75259c3306dd 651 } else {
tashworth 6:75259c3306dd 652 return 0;
tashworth 6:75259c3306dd 653 }
tashworth 0:1b64a0cedc5d 654 }
tashworth 0:1b64a0cedc5d 655
tashworth 8:77a57909aa15 656 void errFunction(void)
tashworth 8:77a57909aa15 657 {
tashworth 8:77a57909aa15 658 //Nothing
tashworth 8:77a57909aa15 659 }
tashworth 8:77a57909aa15 660
tashworth 8:77a57909aa15 661 void us_distance(void)
tashworth 8:77a57909aa15 662 {
tashworth 8:77a57909aa15 663 pc.printf("Ultra Sonic\n\r");
tashworth 8:77a57909aa15 664 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 665 wait_us(20);
tashworth 8:77a57909aa15 666 if ( (rangeFinderLeft.getMeas(range) == RANGE_MEAS_VALID)) {
tashworth 8:77a57909aa15 667 pc.printf("Range = %f\n\r", range);
tashworth 8:77a57909aa15 668 }
tashworth 8:77a57909aa15 669 }
tashworth 8:77a57909aa15 670
tashworth 8:77a57909aa15 671 float wall_follow(int side, int direction, int section)
tashworth 8:77a57909aa15 672 {
tashworth 8:77a57909aa15 673 float location, wavegap=0, set=5;
tashworth 8:77a57909aa15 674 int dir=1;
tashworth 8:77a57909aa15 675
tashworth 8:77a57909aa15 676 pid1.reset();
tashworth 8:77a57909aa15 677
tashworth 8:77a57909aa15 678 if(direction == BACKWARD) dir=-1;
tashworth 8:77a57909aa15 679 if(section == TOOLS)set= 10;
tashworth 8:77a57909aa15 680
tashworth 8:77a57909aa15 681 leftEncoder.reset();
tashworth 8:77a57909aa15 682 rightEncoder.reset();
tashworth 8:77a57909aa15 683
tashworth 8:77a57909aa15 684 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 685
tashworth 9:1b29cd9ed1ba 686 while(location< 68) {
tashworth 8:77a57909aa15 687 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 688
tashworth 8:77a57909aa15 689 pid1.setInputLimits(0, set);
tashworth 8:77a57909aa15 690 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
tashworth 8:77a57909aa15 691 pid1.setSetPoint(set);
tashworth 8:77a57909aa15 692 if(side) {
tashworth 8:77a57909aa15 693 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 694 wait_ms(38);
tashworth 8:77a57909aa15 695 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 696 } else {
tashworth 8:77a57909aa15 697 rangeFinderRight.startMeas();
tashworth 8:77a57909aa15 698 wait_ms(38);
tashworth 8:77a57909aa15 699 rangeFinderRight.getMeas(range);
tashworth 8:77a57909aa15 700 pc.printf("%d\r\n",range);
tashworth 8:77a57909aa15 701 }
tashworth 8:77a57909aa15 702
tashworth 8:77a57909aa15 703 if(range > 20) {
tashworth 8:77a57909aa15 704 wavegap=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 705 //pc.printf("wavegap %f\r\n",wavegap);
tashworth 8:77a57909aa15 706 // AT WAVE OPENING!!!!
tashworth 8:77a57909aa15 707 motors.setMotor1Speed(dir*0.4*127);//left
tashworth 8:77a57909aa15 708 motors.setMotor0Speed(dir*0.4*127);//right
tashworth 8:77a57909aa15 709 } else {
tashworth 8:77a57909aa15 710
tashworth 8:77a57909aa15 711 pid1.setProcessValue(range);
tashworth 8:77a57909aa15 712 pid_return = pid1.compute();
tashworth 8:77a57909aa15 713
tashworth 8:77a57909aa15 714 if(pid_return > 0) {
tashworth 8:77a57909aa15 715 if(side) {
tashworth 8:77a57909aa15 716 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
tashworth 8:77a57909aa15 717 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 718 } else {
tashworth 8:77a57909aa15 719 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
tashworth 8:77a57909aa15 720 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 721 }
tashworth 8:77a57909aa15 722 } else if(pid_return < 0) {
tashworth 8:77a57909aa15 723 if(side) {
tashworth 8:77a57909aa15 724 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 725 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
tashworth 8:77a57909aa15 726 } else {
tashworth 8:77a57909aa15 727 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 728 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
tashworth 8:77a57909aa15 729 }
tashworth 8:77a57909aa15 730 } else {
tashworth 8:77a57909aa15 731 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 732 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 733 }
tashworth 8:77a57909aa15 734 }
tashworth 8:77a57909aa15 735 }
tashworth 8:77a57909aa15 736 return wavegap;
tashworth 8:77a57909aa15 737 }
tashworth 8:77a57909aa15 738
tashworth 8:77a57909aa15 739 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
tashworth 8:77a57909aa15 740
tashworth 8:77a57909aa15 741 void wall_follow2(int side, int direction, int section, float location)
tashworth 8:77a57909aa15 742 {
tashworth 8:77a57909aa15 743 int SeeWaveGap = false, dir=1;
tashworth 8:77a57909aa15 744 float set=5, loc=0;
tashworth 8:77a57909aa15 745
tashworth 8:77a57909aa15 746 pid1.reset();
tashworth 8:77a57909aa15 747
tashworth 8:77a57909aa15 748 if(direction == BACKWARD) dir=-1;
tashworth 8:77a57909aa15 749 if(section == TOOLS)set= 5;
tashworth 8:77a57909aa15 750
tashworth 8:77a57909aa15 751 leftEncoder.reset();
tashworth 8:77a57909aa15 752 rightEncoder.reset();
tashworth 8:77a57909aa15 753
tashworth 8:77a57909aa15 754 while(dir*loc + location <= 78) {
tashworth 8:77a57909aa15 755 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 756
tashworth 8:77a57909aa15 757 pid1.setInputLimits(0.0, set);
tashworth 8:77a57909aa15 758 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
tashworth 8:77a57909aa15 759 pid1.setSetPoint(set);
tashworth 8:77a57909aa15 760
tashworth 8:77a57909aa15 761 if(side) {
tashworth 8:77a57909aa15 762 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 763 wait_ms(38);
tashworth 8:77a57909aa15 764 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 765 } else {
tashworth 8:77a57909aa15 766 rangeFinderRight.startMeas();
tashworth 8:77a57909aa15 767 wait_ms(38);
tashworth 8:77a57909aa15 768 rangeFinderRight.getMeas(range);
tashworth 8:77a57909aa15 769 }
tashworth 8:77a57909aa15 770
tashworth 8:77a57909aa15 771
tashworth 8:77a57909aa15 772 /*************CHECK FOR WAVE OPENING*****************/
tashworth 8:77a57909aa15 773 /* If after 20 ms the ultrasonic still sees 20+ cm */
tashworth 8:77a57909aa15 774 /* then robot is at wave opening */
tashworth 8:77a57909aa15 775
tashworth 8:77a57909aa15 776 //pc.printf("wall follow 2 range %f\r\n",range);
tashworth 8:77a57909aa15 777 //pc.printf("loc+location = %f\r\n", loc+location);
tashworth 8:77a57909aa15 778 if(range > 20) {
tashworth 8:77a57909aa15 779 motors.stopBothMotors();
tashworth 8:77a57909aa15 780 pc.printf("wavegap\r\n");
tashworth 8:77a57909aa15 781 // AT WAVE OPENING!!!!
tashworth 8:77a57909aa15 782 break;
tashworth 8:77a57909aa15 783 }
tashworth 8:77a57909aa15 784
tashworth 8:77a57909aa15 785 pid1.setProcessValue(range);
tashworth 8:77a57909aa15 786 pid_return = pid1.compute();
tashworth 8:77a57909aa15 787 //pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
tashworth 8:77a57909aa15 788
tashworth 8:77a57909aa15 789 if(pid_return > 0) {
tashworth 8:77a57909aa15 790 if(side) {
tashworth 8:77a57909aa15 791 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
tashworth 8:77a57909aa15 792 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 793 } else {
tashworth 8:77a57909aa15 794 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
tashworth 8:77a57909aa15 795 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 796 }
tashworth 8:77a57909aa15 797 } else if(pid_return < 0) {
tashworth 8:77a57909aa15 798 if(side) {
tashworth 8:77a57909aa15 799 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 8:77a57909aa15 800 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
tashworth 8:77a57909aa15 801 } else {
tashworth 8:77a57909aa15 802 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 8:77a57909aa15 803 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
tashworth 8:77a57909aa15 804 }
tashworth 8:77a57909aa15 805 } else {
tashworth 8:77a57909aa15 806 motors.setMotor0Speed(dir*MAX_SPEED);
tashworth 8:77a57909aa15 807 motors.setMotor1Speed(dir*MAX_SPEED);
tashworth 8:77a57909aa15 808 }
tashworth 8:77a57909aa15 809 }
tashworth 8:77a57909aa15 810 motors.stopBothMotors();
tashworth 8:77a57909aa15 811 }
tashworth 0:1b64a0cedc5d 812
tashworth 0:1b64a0cedc5d 813
tashworth 8:77a57909aa15 814 void alignWithWall(int section)
tashworth 8:77a57909aa15 815 {
tashworth 8:77a57909aa15 816 float usValue = 0;
tashworth 8:77a57909aa15 817
tashworth 8:77a57909aa15 818 if(section == TOOLS) {
tashworth 8:77a57909aa15 819 // turn at an angle
tashworth 8:77a57909aa15 820 leftEncoder.reset();
tashworth 8:77a57909aa15 821 rightEncoder.reset();
tashworth 8:77a57909aa15 822 motors.setMotor0Speed(-1.2*MAX_SPEED); //right
tashworth 8:77a57909aa15 823 motors.setMotor1Speed(0.4*MAX_SPEED); //left
tashworth 8:77a57909aa15 824 while(rightEncoder.getPulses()>-1000);
tashworth 8:77a57909aa15 825 motors.stopBothMotors();
tashworth 8:77a57909aa15 826
tashworth 8:77a57909aa15 827 //go backwards toward wall
tashworth 8:77a57909aa15 828 leftEncoder.reset();
tashworth 8:77a57909aa15 829 rightEncoder.reset();
tashworth 8:77a57909aa15 830 motors.setMotor0Speed(-MAX_SPEED); //right
tashworth 8:77a57909aa15 831 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 8:77a57909aa15 832 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
tashworth 8:77a57909aa15 833
tashworth 8:77a57909aa15 834 motors.stopBothMotors();
tashworth 8:77a57909aa15 835
tashworth 8:77a57909aa15 836 // turn left towards wall
tashworth 8:77a57909aa15 837 leftEncoder.reset();
tashworth 8:77a57909aa15 838 rightEncoder.reset();
tashworth 8:77a57909aa15 839 motors.setMotor0Speed(MAX_SPEED); //right
tashworth 8:77a57909aa15 840 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 8:77a57909aa15 841 while(rightEncoder.getPulses() < 10 || abs(leftEncoder.getPulses()) < 10);
tashworth 8:77a57909aa15 842
tashworth 8:77a57909aa15 843 motors.stopBothMotors();
tashworth 8:77a57909aa15 844
tashworth 8:77a57909aa15 845 motors.setMotor0Speed(0.7*MAX_SPEED); //right
tashworth 8:77a57909aa15 846 motors.setMotor1Speed(-0.7*MAX_SPEED); //left
tashworth 8:77a57909aa15 847 } else {
tashworth 8:77a57909aa15 848 rightTurn();
tashworth 8:77a57909aa15 849 motors.setMotor0Speed(-0.7*MAX_SPEED); //right
tashworth 8:77a57909aa15 850 motors.setMotor1Speed(0.7*MAX_SPEED); //left
tashworth 8:77a57909aa15 851 }
tashworth 8:77a57909aa15 852
tashworth 8:77a57909aa15 853 usValue = 0;
tashworth 8:77a57909aa15 854 while(1) {
tashworth 8:77a57909aa15 855 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 856 wait_ms(20);
tashworth 8:77a57909aa15 857 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 858 //pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
tashworth 8:77a57909aa15 859 if(range > usValue && usValue != 0 && range < 25) {
tashworth 8:77a57909aa15 860 break;
tashworth 8:77a57909aa15 861 } else {
tashworth 8:77a57909aa15 862 usValue = range;
tashworth 8:77a57909aa15 863 }
tashworth 8:77a57909aa15 864 }
tashworth 8:77a57909aa15 865 motors.stopBothMotors();
tashworth 8:77a57909aa15 866 }
tashworth 8:77a57909aa15 867
tashworth 8:77a57909aa15 868 void rightTurn(void)
tashworth 8:77a57909aa15 869 {
tashworth 8:77a57909aa15 870 motors.begin();
tashworth 8:77a57909aa15 871 leftEncoder.reset();
tashworth 8:77a57909aa15 872 rightEncoder.reset();
tashworth 8:77a57909aa15 873 motors.setMotor0Speed(-0.5*127);//right
tashworth 8:77a57909aa15 874 motors.setMotor1Speed(0.5*127);//left
tashworth 8:77a57909aa15 875 while(leftEncoder.getPulses()<900 || rightEncoder.getPulses()>-900);
tashworth 8:77a57909aa15 876 motors.stopBothMotors();
tashworth 8:77a57909aa15 877 }
tashworth 8:77a57909aa15 878
tashworth 8:77a57909aa15 879 void leftTurn(void)
tashworth 8:77a57909aa15 880 {
tashworth 8:77a57909aa15 881 /*
tashworth 8:77a57909aa15 882 leftEncoder.reset();
tashworth 8:77a57909aa15 883 rightEncoder.reset();
tashworth 8:77a57909aa15 884 motors.setMotor0Speed(0.4*MAX_SPEED); //right
tashworth 8:77a57909aa15 885 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 8:77a57909aa15 886 while(abs(leftEncoder.getPulses())<2500);
tashworth 8:77a57909aa15 887 motors.stopBothMotors();
tashworth 8:77a57909aa15 888 */
tashworth 8:77a57909aa15 889 motors.begin();
tashworth 8:77a57909aa15 890 leftEncoder.reset();
tashworth 8:77a57909aa15 891 rightEncoder.reset();
tashworth 8:77a57909aa15 892 motors.setMotor0Speed(0.5*127);// right
tashworth 8:77a57909aa15 893 motors.setMotor1Speed(-0.5*127);// left
tashworth 8:77a57909aa15 894 while(abs(leftEncoder.getPulses())<1000 || rightEncoder.getPulses()<1000);
tashworth 8:77a57909aa15 895 motors.stopBothMotors();
tashworth 8:77a57909aa15 896 }
tashworth 8:77a57909aa15 897 void slightleft(void)
tashworth 8:77a57909aa15 898 {
tashworth 8:77a57909aa15 899
tashworth 8:77a57909aa15 900 leftEncoder.reset();
tashworth 8:77a57909aa15 901 rightEncoder.reset();
tashworth 8:77a57909aa15 902 motors.setMotor0Speed(0.5*127);// right
tashworth 8:77a57909aa15 903 motors.setMotor1Speed(-0.5*127);// left
tashworth 8:77a57909aa15 904 while(abs(leftEncoder.getPulses())<50 || rightEncoder.getPulses()<50);
tashworth 8:77a57909aa15 905 motors.stopBothMotors();
tashworth 8:77a57909aa15 906 }
tashworth 8:77a57909aa15 907
tashworth 8:77a57909aa15 908
tashworth 8:77a57909aa15 909 void overBump(int section)
tashworth 8:77a57909aa15 910 {
tashworth 8:77a57909aa15 911 int preLeft=5000, preRight=5000, out=0;
tashworth 8:77a57909aa15 912
tashworth 8:77a57909aa15 913 motors.begin();
tashworth 8:77a57909aa15 914
tashworth 8:77a57909aa15 915 leftEncoder.reset();
tashworth 8:77a57909aa15 916 rightEncoder.reset();
tashworth 8:77a57909aa15 917 motors.setMotor0Speed(-0.2*127); //right
tashworth 8:77a57909aa15 918 motors.setMotor1Speed(-0.2*127); //left
tashworth 8:77a57909aa15 919 while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses())< 50);
tashworth 8:77a57909aa15 920 motors.stopBothMotors();
tashworth 8:77a57909aa15 921
tashworth 8:77a57909aa15 922 leftEncoder.reset();
tashworth 8:77a57909aa15 923 rightEncoder.reset();
tashworth 8:77a57909aa15 924 motors.setMotor0Speed(0.2*127); //right
tashworth 8:77a57909aa15 925 motors.setMotor1Speed(0.2*127); //left
tashworth 9:1b29cd9ed1ba 926 while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0) {
tashworth 8:77a57909aa15 927 preLeft=leftEncoder.getPulses();
tashworth 8:77a57909aa15 928 preRight=rightEncoder.getPulses();
tashworth 8:77a57909aa15 929 wait_ms(100);
tashworth 8:77a57909aa15 930 //pc.printf(" first while left %d right %d \r\n", preLeft, preRight);
tashworth 8:77a57909aa15 931 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
tashworth 8:77a57909aa15 932 }
tashworth 8:77a57909aa15 933
tashworth 8:77a57909aa15 934 motors.stopBothMotors();
tashworth 8:77a57909aa15 935 motors.begin();
tashworth 8:77a57909aa15 936 wait(2);
tashworth 8:77a57909aa15 937 /*
tashworth 8:77a57909aa15 938 motors.stopBothMotors();
tashworth 8:77a57909aa15 939 motors.setMotor0Speed(0.15*127); //right
tashworth 8:77a57909aa15 940 motors.setMotor1Speed(0.15*127); //left
tashworth 8:77a57909aa15 941 preLeft=preRight=5000 ;
tashworth 8:77a57909aa15 942 leftEncoder.reset();
tashworth 8:77a57909aa15 943 rightEncoder.reset();
tashworth 8:77a57909aa15 944 */
tashworth 9:1b29cd9ed1ba 945 // while(/*(abs(leftEncoder.getPulses()) < 1000 || abs(rightEncoder.getPulses())< 1000)*/ IR.getIRDistance() >20 && preLeft!=0){
tashworth 8:77a57909aa15 946 /* preLeft=leftEncoder.getPulses();
tashworth 8:77a57909aa15 947 preRight=rightEncoder.getPulses();
tashworth 8:77a57909aa15 948 pc.printf("second while left %d right %d \r\n", preLeft, preRight);
tashworth 8:77a57909aa15 949 wait_ms(200);
tashworth 8:77a57909aa15 950 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
tashworth 8:77a57909aa15 951 }*/
tashworth 8:77a57909aa15 952
tashworth 8:77a57909aa15 953 leftEncoder.reset();
tashworth 8:77a57909aa15 954 rightEncoder.reset();
tashworth 8:77a57909aa15 955 motors.setMotor0Speed(0.3*127); //right
tashworth 8:77a57909aa15 956 motors.setMotor1Speed(0.3*127); //left
tashworth 8:77a57909aa15 957
tashworth 8:77a57909aa15 958 while(!out) {
tashworth 8:77a57909aa15 959 preLeft=leftEncoder.getPulses();
tashworth 8:77a57909aa15 960 preRight=rightEncoder.getPulses();
tashworth 8:77a57909aa15 961
tashworth 8:77a57909aa15 962 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 963 rangeFinderRight.startMeas();
tashworth 8:77a57909aa15 964 wait_ms(20);
tashworth 8:77a57909aa15 965 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 966 rangeFinderRight.getMeas(range2);
tashworth 8:77a57909aa15 967 if(range < 10 || range2 < 10) out=1;
tashworth 8:77a57909aa15 968
tashworth 8:77a57909aa15 969 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) {
tashworth 8:77a57909aa15 970 motors.setMotor0Speed(0.4*127); //right
tashworth 8:77a57909aa15 971 motors.setMotor1Speed(0.4*127); //left
tashworth 8:77a57909aa15 972 }
tashworth 8:77a57909aa15 973 if(abs(leftEncoder.getPulses()) <1000 || abs(leftEncoder.getPulses())<1000) out=1;
tashworth 8:77a57909aa15 974 }
tashworth 8:77a57909aa15 975
tashworth 8:77a57909aa15 976 motors.stopBothMotors();
tashworth 8:77a57909aa15 977 wait(2);
tashworth 8:77a57909aa15 978 motors.begin();
tashworth 8:77a57909aa15 979
tashworth 8:77a57909aa15 980 preLeft=preRight=5000 ;
tashworth 8:77a57909aa15 981 leftEncoder.reset();
tashworth 8:77a57909aa15 982 rightEncoder.reset();
tashworth 8:77a57909aa15 983 motors.setMotor0Speed(.25*127); //right
tashworth 8:77a57909aa15 984 motors.setMotor1Speed(.25*127); //left
tashworth 8:77a57909aa15 985
tashworth 8:77a57909aa15 986 if(section == TOOLS || section == MID) {
tashworth 9:1b29cd9ed1ba 987 while(IR.getIRDistance() > 20 ) {
tashworth 9:1b29cd9ed1ba 988 //pc.printf("IR %f\r\n", IR.getIRDistance());
tashworth 8:77a57909aa15 989 //pc.printf("third while left %d right %d \r\n", preLeft, preRight);
tashworth 8:77a57909aa15 990 }
tashworth 8:77a57909aa15 991 } else while((abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses())< 200));
tashworth 8:77a57909aa15 992
tashworth 8:77a57909aa15 993 motors.setMotor0Speed(-.25*127); //right
tashworth 8:77a57909aa15 994 motors.setMotor1Speed(-.25*127); //left
tashworth 8:77a57909aa15 995 wait_ms(10);
tashworth 8:77a57909aa15 996 motors.stopBothMotors();
tashworth 8:77a57909aa15 997 wait(2);
tashworth 8:77a57909aa15 998 motors.begin();
tashworth 8:77a57909aa15 999
tashworth 8:77a57909aa15 1000 }
tashworth 8:77a57909aa15 1001
tashworth 8:77a57909aa15 1002 void to_tools_section(float* location, float &current)
tashworth 8:77a57909aa15 1003 {
tashworth 8:77a57909aa15 1004 wall_follow(LEFT,FORWARD, TOOLS);
tashworth 8:77a57909aa15 1005 // current position in reference to the starting position
tashworth 8:77a57909aa15 1006 current=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 8:77a57909aa15 1007 pc.printf("current %f \r\n",current);
tashworth 9:1b29cd9ed1ba 1008
tashworth 8:77a57909aa15 1009 motors.stopBothMotors();
tashworth 8:77a57909aa15 1010
tashworth 9:1b29cd9ed1ba 1011 wait(2);
tashworth 9:1b29cd9ed1ba 1012
tashworth 9:1b29cd9ed1ba 1013 motors.setMotor0Speed(.2*127); //right
tashworth 9:1b29cd9ed1ba 1014 motors.setMotor1Speed(.2*127); //left
tashworth 9:1b29cd9ed1ba 1015 while(IR.getIRDistance()>16);
tashworth 9:1b29cd9ed1ba 1016
tashworth 9:1b29cd9ed1ba 1017 motors.setMotor0Speed(-.2*127); //right
tashworth 9:1b29cd9ed1ba 1018 motors.setMotor1Speed(-.2*127); //left
tashworth 9:1b29cd9ed1ba 1019 wait_ms(5);
tashworth 9:1b29cd9ed1ba 1020 motors.stopBothMotors();
tashworth 8:77a57909aa15 1021
tashworth 8:77a57909aa15 1022 }
tashworth 8:77a57909aa15 1023
tashworth 8:77a57909aa15 1024 void from_tools_section(float* location, float &current)
tashworth 8:77a57909aa15 1025 {
tashworth 8:77a57909aa15 1026 alignWithWall(TOOLS);
tashworth 8:77a57909aa15 1027
tashworth 8:77a57909aa15 1028 wait_ms(100);
tashworth 8:77a57909aa15 1029
tashworth 8:77a57909aa15 1030 wall_follow2(LEFT,FORWARD,MID, current);
tashworth 8:77a57909aa15 1031 current= 78;
tashworth 8:77a57909aa15 1032
tashworth 8:77a57909aa15 1033 rangeFinderLeft.startMeas();
tashworth 8:77a57909aa15 1034 wait_ms(20);
tashworth 8:77a57909aa15 1035 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 1036
tashworth 8:77a57909aa15 1037 if(range < 20) {
tashworth 8:77a57909aa15 1038 wall_follow2(LEFT,BACKWARD,TOOLS, current);
tashworth 8:77a57909aa15 1039 leftEncoder.reset();
tashworth 8:77a57909aa15 1040 rightEncoder.reset();
tashworth 8:77a57909aa15 1041 motors.setMotor0Speed(-MAX_SPEED); //right
tashworth 8:77a57909aa15 1042 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 8:77a57909aa15 1043 while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
tashworth 8:77a57909aa15 1044
tashworth 8:77a57909aa15 1045 leftEncoder.reset();
tashworth 8:77a57909aa15 1046 rightEncoder.reset();
tashworth 8:77a57909aa15 1047 motors.setMotor0Speed(MAX_SPEED); //right
tashworth 8:77a57909aa15 1048 motors.setMotor1Speed(MAX_SPEED); //left
tashworth 8:77a57909aa15 1049 while(abs(leftEncoder.getPulses()) < 40 || abs(rightEncoder.getPulses())< 40);
tashworth 8:77a57909aa15 1050 motors.stopBothMotors();
tashworth 8:77a57909aa15 1051
tashworth 8:77a57909aa15 1052 wait_ms(500);
tashworth 8:77a57909aa15 1053 location[0]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1054 current= location[0];
tashworth 8:77a57909aa15 1055 leftTurn();
tashworth 8:77a57909aa15 1056 slightleft();
tashworth 8:77a57909aa15 1057 overBump(TOOLS);
tashworth 8:77a57909aa15 1058 } else {
tashworth 8:77a57909aa15 1059 location[0]= 77;
tashworth 8:77a57909aa15 1060 leftTurn();
tashworth 8:77a57909aa15 1061 wait_ms(20);
tashworth 8:77a57909aa15 1062 overBump(FIRST_WAVE);
tashworth 8:77a57909aa15 1063 }
tashworth 8:77a57909aa15 1064
tashworth 8:77a57909aa15 1065 pc.printf("First Wavegap = %f\r\n",location[0]);
tashworth 8:77a57909aa15 1066 }
tashworth 8:77a57909aa15 1067 void mid_section(float* location, float &current, int* direction)
tashworth 8:77a57909aa15 1068 {
tashworth 8:77a57909aa15 1069
tashworth 8:77a57909aa15 1070 motors.begin();
tashworth 8:77a57909aa15 1071
tashworth 9:1b29cd9ed1ba 1072 if(IR.getIRDistance() > 20) return;
tashworth 8:77a57909aa15 1073
tashworth 8:77a57909aa15 1074 alignWithWall(MID);
tashworth 8:77a57909aa15 1075
tashworth 8:77a57909aa15 1076 pc.printf("mid section current = %f\r\n",current);
tashworth 8:77a57909aa15 1077 wall_follow2(LEFT,FORWARD,MID, current);
tashworth 8:77a57909aa15 1078 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1079 pc.printf("after wf2 current = %f\r\n",current);
tashworth 8:77a57909aa15 1080
tashworth 8:77a57909aa15 1081 if(current != 0) {
tashworth 8:77a57909aa15 1082 direction[0]= RIGHT;
tashworth 8:77a57909aa15 1083 current+= location[0];
tashworth 8:77a57909aa15 1084 location[1]= current;
tashworth 8:77a57909aa15 1085 } else {
tashworth 8:77a57909aa15 1086 current=location[0];
tashworth 8:77a57909aa15 1087 direction[0]= LEFT;
tashworth 8:77a57909aa15 1088 wall_follow2(LEFT,BACKWARD,MID,current);
tashworth 8:77a57909aa15 1089 location[1]= location[0]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1090 }
tashworth 8:77a57909aa15 1091
tashworth 8:77a57909aa15 1092 pc.printf("wavegap2 = %f\r\n",location[1]);
tashworth 8:77a57909aa15 1093 leftTurn();
tashworth 8:77a57909aa15 1094 overBump(TOOLS);
tashworth 8:77a57909aa15 1095 // go forward
tashworth 8:77a57909aa15 1096 leftEncoder.reset();
tashworth 8:77a57909aa15 1097 rightEncoder.reset();
tashworth 8:77a57909aa15 1098 motors.setMotor0Speed(0.2*127); //right
tashworth 8:77a57909aa15 1099 motors.setMotor1Speed(0.2*127); //left
tashworth 8:77a57909aa15 1100 while(abs(leftEncoder.getPulses())<300 || abs(rightEncoder.getPulses())<300);
tashworth 8:77a57909aa15 1101 motors.stopBothMotors();
tashworth 8:77a57909aa15 1102
tashworth 8:77a57909aa15 1103 }
tashworth 8:77a57909aa15 1104
tashworth 8:77a57909aa15 1105 void mid_section2(float* location, float &current, int* direction)
tashworth 8:77a57909aa15 1106 {
tashworth 8:77a57909aa15 1107
tashworth 8:77a57909aa15 1108 motors.begin();
tashworth 8:77a57909aa15 1109
tashworth 9:1b29cd9ed1ba 1110 if(IR.getIRDistance() > 20) return;
tashworth 8:77a57909aa15 1111
tashworth 8:77a57909aa15 1112 alignWithWall(MID);
tashworth 8:77a57909aa15 1113 wall_follow2(LEFT,FORWARD,MID, current);
tashworth 8:77a57909aa15 1114 current=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1115
tashworth 8:77a57909aa15 1116 if(current != 0) {
tashworth 8:77a57909aa15 1117 direction[1]= RIGHT;
tashworth 8:77a57909aa15 1118 current+= location[1];
tashworth 8:77a57909aa15 1119 location[2]= current;
tashworth 8:77a57909aa15 1120 } else {
tashworth 8:77a57909aa15 1121 current=location[1];
tashworth 8:77a57909aa15 1122 direction[1]= LEFT;
tashworth 8:77a57909aa15 1123 wall_follow2(LEFT,BACKWARD,MID,current);
tashworth 8:77a57909aa15 1124 location[2]= location[1]- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 8:77a57909aa15 1125 }
tashworth 8:77a57909aa15 1126
tashworth 8:77a57909aa15 1127 leftTurn();
tashworth 8:77a57909aa15 1128 overBump(RIGS);
tashworth 8:77a57909aa15 1129 }
tashworth 8:77a57909aa15 1130
tashworth 8:77a57909aa15 1131 void rig_section(float* location, float &current, int* direction, int rig)
tashworth 8:77a57909aa15 1132 {
tashworth 8:77a57909aa15 1133
tashworth 8:77a57909aa15 1134
tashworth 8:77a57909aa15 1135 }
tashworth 8:77a57909aa15 1136
tashworth 9:1b29cd9ed1ba 1137 void ledtoggle(void)
tashworth 9:1b29cd9ed1ba 1138 {
tashworth 8:77a57909aa15 1139 pwm.setPWM(9, 0, 4094);
tashworth 8:77a57909aa15 1140 wait(2);
tashworth 8:77a57909aa15 1141 pwm.setPWM(9, 0, 0);
tashworth 9:1b29cd9ed1ba 1142 }
tashworth 8:77a57909aa15 1143
tashworth 8:77a57909aa15 1144
tashworth 8:77a57909aa15 1145