Thomas Ashworth / theRobot

For IEEE Robotics

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

main.cpp@8:77a57909aa15, 2014-03-27 (annotated)

Committer:
tashworth
Date:
Thu Mar 27 23:16:02 2014 +0000
Revision:
8:77a57909aa15
Parent:
7:8fb4204f9600
Child:
9:1b29cd9ed1ba
3-27-14

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