Thomas Ashworth / theRobot

For IEEE Robotics

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

main.cpp@21:0907e1f5e16c, 2014-04-08 (annotated)

Committer:
tashworth
Date:
Tue Apr 08 16:20:40 2014 +0000
Revision:
21:0907e1f5e16c
Parent:
20:55dcff40c5d9
Child:
22:79c5871543b5
4-8-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 20:55dcff40c5d9 13
tashworth 20:55dcff40c5d9 14 #define PI 3.14159
tashworth 20:55dcff40c5d9 15
tashworth 20:55dcff40c5d9 16
tashworth 20:55dcff40c5d9 17
tashworth 8:77a57909aa15 18 /* Navigation Definitions */
tashworth 8:77a57909aa15 19 #define PIN_TRIGGERL (p12)
tashworth 8:77a57909aa15 20 #define PIN_ECHOL (p11)
tashworth 8:77a57909aa15 21 #define PIN_TRIGGERR (p29)
tashworth 8:77a57909aa15 22 #define PIN_ECHOR (p30)
tashworth 8:77a57909aa15 23 #define PULSE_PER_REV (1192)
tashworth 8:77a57909aa15 24 #define WHEEL_CIRCUM (12.56637)
tashworth 8:77a57909aa15 25 #define DIST_PER_PULSE (0.01054225722682)
tashworth 8:77a57909aa15 26 #define MTRS_TO_INCH (39.3701)
tashworth 8:77a57909aa15 27 #define MAX_SPEED (0.3*127)
tashworth 20:55dcff40c5d9 28 #define MAX_SPEED1 (0.25*127)
tashworth 8:77a57909aa15 29 #define PPR (4331/4)
tashworth 8:77a57909aa15 30 #define LEFT (1)
tashworth 8:77a57909aa15 31 #define RIGHT (0)
tashworth 12:284be46593ae 32 #define STRAIGHT (2)
tashworth 8:77a57909aa15 33 #define FORWARD (1)
tashworth 8:77a57909aa15 34 #define BACKWARD (0)
tashworth 8:77a57909aa15 35 #define TOOLS (0)
tashworth 8:77a57909aa15 36 #define MID (1)
tashworth 8:77a57909aa15 37 #define RIGS (2)
tashworth 12:284be46593ae 38 #define MID2 (3)
tashworth 12:284be46593ae 39 #define RETURN (4)
tashworth 8:77a57909aa15 40 #define FAR (1)
tashworth 6:75259c3306dd 41
tashworth 20:55dcff40c5d9 42
tashworth 6:75259c3306dd 43 //States
tashworth 6:75259c3306dd 44 #define START 0
tashworth 6:75259c3306dd 45 #define OILRIG1_POS 1
tashworth 6:75259c3306dd 46 #define OILRIG2_POS 2
tashworth 12:284be46593ae 47 #define GOTO_TOOLS1 3
tashworth 6:75259c3306dd 48 #define IDENTIFY_TOOLS 4
tashworth 6:75259c3306dd 49 #define AQUIRE_TOOL1 5
tashworth 6:75259c3306dd 50 #define AQUIRE_TOOL2 6
tashworth 6:75259c3306dd 51 #define AQUIRE_TOOL3 7
tashworth 6:75259c3306dd 52 #define NAVIGATE_WAVES_ROW1 8
tashworth 6:75259c3306dd 53 #define NAVIGATE_WAVES_ROW2 9
tashworth 6:75259c3306dd 54 #define NAVIGATE_WAVES_ROW3 10
tashworth 6:75259c3306dd 55 #define NAVIGATE_TO_SQUARE_RIG 11
tashworth 6:75259c3306dd 56 #define NAVIGATE_TO_TRIANGLE_RIG 12
tashworth 6:75259c3306dd 57 #define NAVIGATE_TO_CIRCLE_RIG 13
tashworth 6:75259c3306dd 58 #define RIG_ALIGN 14
tashworth 6:75259c3306dd 59 #define INSERT_TOOL 15
tashworth 6:75259c3306dd 60 #define END 16
tashworth 12:284be46593ae 61 #define GOTO_TOOLS2 17
tashworth 6:75259c3306dd 62
tashworth 0:1b64a0cedc5d 63
tashworth 8:77a57909aa15 64
tashworth 6:75259c3306dd 65 //Servo Static Positions
tashworth 6:75259c3306dd 66 #define STORE_POSITION 0
tashworth 6:75259c3306dd 67 #define OIL_RIG1 1
tashworth 6:75259c3306dd 68 #define OIL_RIG2 2
tashworth 6:75259c3306dd 69 #define OIL_RIG3 3
tashworth 6:75259c3306dd 70 #define DRIVE_POSITION_NOTOOL 4
tashworth 6:75259c3306dd 71 #define TOOL_1 5
tashworth 6:75259c3306dd 72 #define TOOL_2 6
tashworth 6:75259c3306dd 73 #define TOOL_3 7
tashworth 6:75259c3306dd 74 #define DRIVE_POSITION_TOOL 8
tashworth 6:75259c3306dd 75 #define ORIENT_TOOL 9
tashworth 11:8d2455e383ce 76 #define PU_TOOL_1 10
tashworth 11:8d2455e383ce 77 #define PU_TOOL_2 11
tashworth 11:8d2455e383ce 78 #define PU_TOOL_3 12
tashworth 11:8d2455e383ce 79 #define PU_TOOL_1_STAB 13
tashworth 11:8d2455e383ce 80 #define PU_TOOL_2_STAB 14
tashworth 11:8d2455e383ce 81 #define PU_TOOL_3_STAB 15
tashworth 2:4e082e4c255d 82
tashworth 6:75259c3306dd 83 //Rig definitions
tashworth 6:75259c3306dd 84 #define SQUARE 1
tashworth 6:75259c3306dd 85 #define TRIANGLE 2
tashworth 6:75259c3306dd 86 #define CIRCLE 3
tashworth 6:75259c3306dd 87
tashworth 17:a5bb85ee205d 88
tashworth 6:75259c3306dd 89 //Oil Rig distance thresholds
tashworth 21:0907e1f5e16c 90 #define OILRIG1_MAX 1800
tashworth 8:77a57909aa15 91 #define OILRIG1_MIN 1000
tashworth 21:0907e1f5e16c 92 #define OILRIG2_MAX 1800
tashworth 8:77a57909aa15 93 #define OILRIG2_MIN 1000
tashworth 21:0907e1f5e16c 94 #define OILRIG3_MAX 1800
tashworth 8:77a57909aa15 95 #define OILRIG3_MIN 1000
tashworth 6:75259c3306dd 96
tashworth 6:75259c3306dd 97 //for servo normalization
tashworth 3:b7b4780a7f6e 98 #define MIN_SERVO_PULSE 900
tashworth 3:b7b4780a7f6e 99 #define MAX_SERVO_PULSE 2100
tashworth 3:b7b4780a7f6e 100 #define SERVO_MAX_ANGLE 180
tashworth 3:b7b4780a7f6e 101
tashworth 17:a5bb85ee205d 102 #define X_CENTER 80
tashworth 17:a5bb85ee205d 103 #define Y_CENTER 60
tashworth 17:a5bb85ee205d 104
tashworth 8:77a57909aa15 105
tashworth 8:77a57909aa15 106 DigitalOut myled1(LED1);
tashworth 8:77a57909aa15 107 DigitalOut myled2(LED2);
tashworth 8:77a57909aa15 108 DigitalOut myled3(LED3);
tashworth 8:77a57909aa15 109 DigitalOut myled4(LED4);
tashworth 16:8bb212df81b7 110 InterruptIn startBtn(p7);
tashworth 8:77a57909aa15 111
tashworth 8:77a57909aa15 112 void errFunction(void);
tashworth 8:77a57909aa15 113 bool cRc;
tashworth 8:77a57909aa15 114
tashworth 8:77a57909aa15 115 Serial pc(USBTX,USBRX); //USB Comm
tashworth 8:77a57909aa15 116 Adafruit_PWMServoDriver pwm(p28,p27); //pwm(SDA,SCL) - Servo Control PWM
tashworth 8:77a57909aa15 117 extern Serial lrf; //Laser Range Finder lrf(p13,p14)
tashworth 8:77a57909aa15 118 //Hardware Initialization
tashworth 8:77a57909aa15 119 //Serial bt(p13,p14);
tashworth 8:77a57909aa15 120 HCSR04 rangeFinderLeft( PIN_TRIGGERL, PIN_ECHOL );
tashworth 8:77a57909aa15 121 HCSR04 rangeFinderRight( PIN_TRIGGERR, PIN_ECHOR );
tashworth 8:77a57909aa15 122 PID pid1(15.0,0.0,4.0,0.02);
tashworth 8:77a57909aa15 123 PololuQik2 motors(p9, p10, p8, p15, errFunction, cRc);
tashworth 8:77a57909aa15 124 QEI rightEncoder(p17,p18,NC,PPR,QEI::X4_ENCODING);
tashworth 8:77a57909aa15 125 QEI leftEncoder(p16,p15,NC,PPR,QEI::X4_ENCODING);
tashworth 8:77a57909aa15 126 Sharp IR(p20);
tashworth 8:77a57909aa15 127 //InterruptIn encoder(p29);
tashworth 8:77a57909aa15 128
tashworth 8:77a57909aa15 129
tashworth 8:77a57909aa15 130
tashworth 8:77a57909aa15 131
tashworth 6:75259c3306dd 132 /***************
tashworth 6:75259c3306dd 133 local servo functions
tashworth 6:75259c3306dd 134 ****************/
tashworth 1:fe4a0b47ff25 135 void servoBegin(void);
tashworth 0:1b64a0cedc5d 136 void initServoDriver(void);
tashworth 3:b7b4780a7f6e 137 void setServoPulse(uint8_t n, int angle);
tashworth 17:a5bb85ee205d 138 void setServoPulse2(uint8_t n, float angle); //float precision
tashworth 3:b7b4780a7f6e 139 void setServoPulseNo_delay(uint8_t n, int angle);
tashworth 0:1b64a0cedc5d 140 void servoPosition(int set);
tashworth 6:75259c3306dd 141 int fire_checker(int rig);
tashworth 16:8bb212df81b7 142 int button_start = 0;
tashworth 6:75259c3306dd 143
tashworth 8:77a57909aa15 144
tashworth 8:77a57909aa15 145 //Navigation Functions
tashworth 12:284be46593ae 146 void wall_follow(int side, int direction, int section);
tashworth 12:284be46593ae 147 void wall_follow2(int side, int direction, int section, float location, int rig);
tashworth 8:77a57909aa15 148 void leftTurn(void);
tashworth 8:77a57909aa15 149 void slightleft(void);
tashworth 12:284be46593ae 150 void slightright(void);
tashworth 8:77a57909aa15 151 void rightTurn(void);
tashworth 12:284be46593ae 152 void slightMove(int direction, float pulses);
tashworth 12:284be46593ae 153 void to_tools_section1(float* location, float &current);
tashworth 12:284be46593ae 154 void to_tools_section2(float* location, float &current);
tashworth 8:77a57909aa15 155 void from_tools_section(float* location, float &current);
tashworth 8:77a57909aa15 156 void mid_section(float* location, float &current, int* direction);
tashworth 8:77a57909aa15 157 void mid_section2(float* location, float &current, int* direction);
tashworth 8:77a57909aa15 158 void rig_section(float* location, float &current, int* direction, int rig);
tashworth 12:284be46593ae 159 void tools_section_return(float* location, float &current);
tashworth 12:284be46593ae 160 void mid_section_return(float* location, float &current, int* direction);
tashworth 12:284be46593ae 161 void mid_section2_return(float* location, float &current, int* direction);
tashworth 12:284be46593ae 162 void rig_section_return(float* location, float &current, int* direction);
tashworth 8:77a57909aa15 163 void overBump(int section);
tashworth 8:77a57909aa15 164 void alignWithWall(int section);
tashworth 12:284be46593ae 165 void UntilWall(int dir);
tashworth 8:77a57909aa15 166
tashworth 20:55dcff40c5d9 167
tashworth 20:55dcff40c5d9 168 float normd(int* pop, int count, int threshold);
tashworth 20:55dcff40c5d9 169 int Xadjust(int tool);
tashworth 20:55dcff40c5d9 170
tashworth 16:8bb212df81b7 171 extern "C" void mbed_reset();
tashworth 8:77a57909aa15 172
tashworth 6:75259c3306dd 173 /************
tashworth 6:75259c3306dd 174 Main Variables
tashworth 6:75259c3306dd 175 *************/
tashworth 6:75259c3306dd 176 int state = START;
tashworth 6:75259c3306dd 177 int fire = 0;
tashworth 20:55dcff40c5d9 178 int tool_needed = 1;
tashworth 6:75259c3306dd 179 int shape_detected = 0;
tashworth 8:77a57909aa15 180 float range, range2, pid_return;
tashworth 19:d4d967a885dc 181 int num, input;
tashworth 8:77a57909aa15 182
tashworth 0:1b64a0cedc5d 183
tashworth 0:1b64a0cedc5d 184 /************
tashworth 0:1b64a0cedc5d 185 Variables for Servos
tashworth 0:1b64a0cedc5d 186 *************/
tashworth 3:b7b4780a7f6e 187 int servoNum, servoAngle, outputDisabled, posNum, testVal;
tashworth 3:b7b4780a7f6e 188 int currentPosition[7];
tashworth 0:1b64a0cedc5d 189
tashworth 3:b7b4780a7f6e 190 typedef struct {
tashworth 6:75259c3306dd 191 int arm_position_name; //for organization only (STORE, OILRIG1, OILRIG2...)
tashworth 3:b7b4780a7f6e 192 int base_rotate;
tashworth 3:b7b4780a7f6e 193 int base_arm;
tashworth 3:b7b4780a7f6e 194 int big_arm;
tashworth 3:b7b4780a7f6e 195 int claw_arm;
tashworth 3:b7b4780a7f6e 196 int claw_rotate;
tashworth 3:b7b4780a7f6e 197 int claw_open;
tashworth 3:b7b4780a7f6e 198 } Coord;
tashworth 3:b7b4780a7f6e 199
tashworth 6:75259c3306dd 200 /********************
tashworth 6:75259c3306dd 201 Static Arm Positions
tashworth 6:75259c3306dd 202 *********************/
tashworth 6:75259c3306dd 203
tashworth 3:b7b4780a7f6e 204 Coord Arm_Table[] = {
tashworth 6:75259c3306dd 205
tashworth 3:b7b4780a7f6e 206 // POSITION ODER:
tashworth 6:75259c3306dd 207 // base_rotate, base_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
tashworth 3:b7b4780a7f6e 208
tashworth 8:77a57909aa15 209 //increase in number 5 rotates gripper
tashworth 8:77a57909aa15 210
tashworth 17:a5bb85ee205d 211 {STORE_POSITION, 85, 10, 0, 165, 175, 0}, // storing position
tashworth 21:0907e1f5e16c 212 {OIL_RIG1, 160, 20, 60, 47, 175, 0}, // point laser at oilrig1
tashworth 21:0907e1f5e16c 213 {OIL_RIG2, 164, 20, 60, 47, 175, 0}, // point laser at oilrig2
tashworth 12:284be46593ae 214 {OIL_RIG3, 130, 90, 90, 52, 175, 0}, // point laser at oilrig2
tashworth 17:a5bb85ee205d 215 {DRIVE_POSITION_NOTOOL, 85, 10, 0, 165, 175, 0}, // Drive through course
tashworth 19:d4d967a885dc 216 {TOOL_1, 101, 50, 80, 113, 90, 0}, // Look over first tool
tashworth 19:d4d967a885dc 217 {TOOL_2, 82, 50, 80, 113, 90, 0}, // Look over second tool
tashworth 19:d4d967a885dc 218 {TOOL_3, 62, 50, 80, 112, 90, 0}, // Look over third tool
tashworth 17:a5bb85ee205d 219 {DRIVE_POSITION_TOOL, 85, 10, 0, 165, 90, 105}, // Drive with tool loaded
tashworth 6:75259c3306dd 220 {ORIENT_TOOL, 135, 60, 75, 60, 90, 90}, // position tool to be inserted
tashworth 19:d4d967a885dc 221 {PU_TOOL_1, 99, 46, 78, 110, 170, 0}, // STATIC Pickup tool POSITION
tashworth 19:d4d967a885dc 222 {PU_TOOL_2, 76, 47, 80, 112, 170, 0}, // STATIC Pickup tool POSITION
tashworth 19:d4d967a885dc 223 {PU_TOOL_3, 59, 44, 76, 110, 170, 0}, // STATIC Pickup tool POSITION
tashworth 11:8d2455e383ce 224 {PU_TOOL_1_STAB, 98, 50, 90, 118, 90, 0}, // STAB TOOL 1
tashworth 11:8d2455e383ce 225 {PU_TOOL_2_STAB, 78, 50, 90, 108, 90, 0}, // STAB TOOL 2
tashworth 11:8d2455e383ce 226 {PU_TOOL_3_STAB, 53, 50, 90, 118, 90, 0}, // STAB TOOL 3
tashworth 3:b7b4780a7f6e 227 };
tashworth 3:b7b4780a7f6e 228
tashworth 3:b7b4780a7f6e 229
tashworth 3:b7b4780a7f6e 230 /* Variables for imageprocessing and distance */
tashworth 3:b7b4780a7f6e 231 int x_coord;
tashworth 3:b7b4780a7f6e 232 int y_coord;
tashworth 3:b7b4780a7f6e 233 int area;
tashworth 3:b7b4780a7f6e 234 int shape;
tashworth 13:529323807361 235 int shape_alignX_done = 0;
tashworth 16:8bb212df81b7 236 int shape_alignY_done = 0;
tashworth 17:a5bb85ee205d 237 float deltaX = 0;
tashworth 13:529323807361 238
tashworth 3:b7b4780a7f6e 239
tashworth 6:75259c3306dd 240 /* Variables for distance sensor*/
tashworth 8:77a57909aa15 241 int distLaser;
tashworth 6:75259c3306dd 242 int fire_detected = 0;
tashworth 6:75259c3306dd 243 int fire_not_detected = 0;
tashworth 3:b7b4780a7f6e 244
tashworth 16:8bb212df81b7 245 void button_int(void)
tashworth 16:8bb212df81b7 246 {
tashworth 16:8bb212df81b7 247 if(!button_start) {
tashworth 16:8bb212df81b7 248 button_start = 1;
tashworth 16:8bb212df81b7 249 wait(1.0);
tashworth 16:8bb212df81b7 250 } else {
tashworth 16:8bb212df81b7 251 button_start = 0;
tashworth 16:8bb212df81b7 252 mbed_reset();
tashworth 16:8bb212df81b7 253 }
tashworth 16:8bb212df81b7 254 return;
tashworth 16:8bb212df81b7 255 }
tashworth 16:8bb212df81b7 256
tashworth 0:1b64a0cedc5d 257
tashworth 19:d4d967a885dc 258
tashworth 3:b7b4780a7f6e 259 int main()
tashworth 3:b7b4780a7f6e 260 {
tashworth 3:b7b4780a7f6e 261
tashworth 19:d4d967a885dc 262
tashworth 3:b7b4780a7f6e 263 /*****************
tashworth 3:b7b4780a7f6e 264 INITIALIZATIONS
tashworth 3:b7b4780a7f6e 265 *******************/
tashworth 12:284be46593ae 266 float location[3], current=4;
tashworth 8:77a57909aa15 267 int direction[3];
tashworth 8:77a57909aa15 268 double distance;
tashworth 8:77a57909aa15 269
tashworth 17:a5bb85ee205d 270 int pu, num, input;
tashworth 17:a5bb85ee205d 271
tashworth 9:1b29cd9ed1ba 272
tashworth 3:b7b4780a7f6e 273 pc.baud(115200);
tashworth 6:75259c3306dd 274 //Laser Range Finder Initialization
tashworth 3:b7b4780a7f6e 275 lrf_baudCalibration();
tashworth 17:a5bb85ee205d 276
tashworth 8:77a57909aa15 277 motors.begin();
tashworth 17:a5bb85ee205d 278
tashworth 16:8bb212df81b7 279 startBtn.rise(&button_int);
tashworth 3:b7b4780a7f6e 280
tashworth 6:75259c3306dd 281 //Servo initialization
tashworth 3:b7b4780a7f6e 282 initServoDriver();
tashworth 6:75259c3306dd 283 servoBegin(); //initiates servos to start position
tashworth 11:8d2455e383ce 284 //ServoOutputDisable = 0;
tashworth 11:8d2455e383ce 285
tashworth 7:8fb4204f9600 286 /********************************
tashworth 7:8fb4204f9600 287 MAIN WHILE LOOP FOR COMPETITION
tashworth 7:8fb4204f9600 288 *********************************/
tashworth 19:d4d967a885dc 289
tashworth 20:55dcff40c5d9 290
tashworth 3:b7b4780a7f6e 291 while(1) {
tashworth 16:8bb212df81b7 292 if(button_start == 1) {
tashworth 10:1a1d52207f59 293
tashworth 16:8bb212df81b7 294
tashworth 16:8bb212df81b7 295 switch (state) {
tashworth 8:77a57909aa15 296
tashworth 16:8bb212df81b7 297 /**************************************************
tashworth 16:8bb212df81b7 298 * STAGE 0
tashworth 16:8bb212df81b7 299 *
tashworth 16:8bb212df81b7 300 * - START OF THE COMETITION
tashworth 16:8bb212df81b7 301 *
tashworth 16:8bb212df81b7 302 **************************************************/
tashworth 16:8bb212df81b7 303 case START :
tashworth 16:8bb212df81b7 304 myled1 = 1;
tashworth 16:8bb212df81b7 305 state = OILRIG1_POS;
tashworth 16:8bb212df81b7 306 break;
tashworth 3:b7b4780a7f6e 307
tashworth 13:529323807361 308
tashworth 16:8bb212df81b7 309 /**************************************************
tashworth 16:8bb212df81b7 310 * STAGE 1
tashworth 16:8bb212df81b7 311 *
tashworth 16:8bb212df81b7 312 * - DETERMINE OIL RIG ON FIRE
tashworth 16:8bb212df81b7 313 *
tashworth 16:8bb212df81b7 314 **************************************************/
tashworth 10:1a1d52207f59 315
tashworth 16:8bb212df81b7 316 case OILRIG1_POS: //aims arm at square oil rig
tashworth 11:8d2455e383ce 317
tashworth 16:8bb212df81b7 318 servoPosition(OIL_RIG1);
tashworth 16:8bb212df81b7 319 wait(3); //wait for servo to settle before laser distance
tashworth 16:8bb212df81b7 320
tashworth 16:8bb212df81b7 321 fire = fire_checker(OIL_RIG1); //determines if oil rig is on fire
tashworth 12:284be46593ae 322
tashworth 16:8bb212df81b7 323 //determines what tool is needed
tashworth 16:8bb212df81b7 324 if (fire == 1) {
tashworth 16:8bb212df81b7 325 pc.printf("FIRE FOUND!!!!!!!!\n\r");
tashworth 16:8bb212df81b7 326 tool_needed = SQUARE;
tashworth 16:8bb212df81b7 327 state = GOTO_TOOLS1;
tashworth 16:8bb212df81b7 328 } else {
tashworth 16:8bb212df81b7 329 pc.printf("XXXXXX FIRE NOT FOUND XXXXXX");
tashworth 16:8bb212df81b7 330 state = OILRIG2_POS;
tashworth 16:8bb212df81b7 331 }
tashworth 16:8bb212df81b7 332 break;
tashworth 16:8bb212df81b7 333
tashworth 16:8bb212df81b7 334 case OILRIG2_POS:
tashworth 6:75259c3306dd 335
tashworth 16:8bb212df81b7 336 setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
tashworth 16:8bb212df81b7 337 setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
tashworth 16:8bb212df81b7 338 setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
tashworth 16:8bb212df81b7 339 setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
tashworth 16:8bb212df81b7 340 setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
tashworth 16:8bb212df81b7 341 setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
tashworth 16:8bb212df81b7 342 wait(3); //wait for servos to settle
tashworth 12:284be46593ae 343
tashworth 16:8bb212df81b7 344 to_tools_section2(location, current); // moves to second rig
tashworth 12:284be46593ae 345
tashworth 16:8bb212df81b7 346 servoPosition(OIL_RIG2); //position arm to point at first oilrig
tashworth 16:8bb212df81b7 347 wait(3);
tashworth 13:529323807361 348
tashworth 16:8bb212df81b7 349 fire = fire_checker(OIL_RIG2);
tashworth 16:8bb212df81b7 350 if (fire == 1) {
tashworth 16:8bb212df81b7 351 pc.printf("FIRE FOUND!!!!!!!!");
tashworth 16:8bb212df81b7 352 tool_needed = TRIANGLE;
tashworth 16:8bb212df81b7 353 state = GOTO_TOOLS2;
tashworth 16:8bb212df81b7 354 } else {
tashworth 16:8bb212df81b7 355 pc.printf("XXXXXX FIRE NOT FOUND XXXXXX");
tashworth 16:8bb212df81b7 356 tool_needed = CIRCLE;
tashworth 16:8bb212df81b7 357 state = GOTO_TOOLS2;
tashworth 16:8bb212df81b7 358 }
tashworth 16:8bb212df81b7 359 break;
tashworth 6:75259c3306dd 360
tashworth 16:8bb212df81b7 361 /**************************************************
tashworth 16:8bb212df81b7 362 * STAGE 2
tashworth 16:8bb212df81b7 363 *
tashworth 16:8bb212df81b7 364 * - TRAVEL TO TOOLS
tashworth 16:8bb212df81b7 365 *
tashworth 16:8bb212df81b7 366 **************************************************/
tashworth 16:8bb212df81b7 367 case GOTO_TOOLS1:
tashworth 16:8bb212df81b7 368 setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
tashworth 16:8bb212df81b7 369 setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
tashworth 16:8bb212df81b7 370 setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
tashworth 16:8bb212df81b7 371 setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
tashworth 16:8bb212df81b7 372 setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
tashworth 16:8bb212df81b7 373 setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
tashworth 16:8bb212df81b7 374 wait(3); //wait for servos to settle
tashworth 16:8bb212df81b7 375 to_tools_section1(location, current);
tashworth 16:8bb212df81b7 376 state = IDENTIFY_TOOLS;
tashworth 16:8bb212df81b7 377 break;
tashworth 12:284be46593ae 378
tashworth 16:8bb212df81b7 379 case GOTO_TOOLS2:
tashworth 12:284be46593ae 380
tashworth 16:8bb212df81b7 381 setServoPulse(2, Arm_Table[DRIVE_POSITION_NOTOOL].big_arm);
tashworth 16:8bb212df81b7 382 setServoPulse(3, Arm_Table[DRIVE_POSITION_NOTOOL].claw_arm);
tashworth 16:8bb212df81b7 383 setServoPulse(1, Arm_Table[DRIVE_POSITION_NOTOOL].base_arm);
tashworth 16:8bb212df81b7 384 setServoPulse(0, Arm_Table[DRIVE_POSITION_NOTOOL].base_rotate);
tashworth 16:8bb212df81b7 385 setServoPulse(4, Arm_Table[DRIVE_POSITION_NOTOOL].claw_rotate);
tashworth 16:8bb212df81b7 386 setServoPulse(5, Arm_Table[DRIVE_POSITION_NOTOOL].claw_open);
tashworth 16:8bb212df81b7 387 wait(3); //wait for servos to settle
tashworth 8:77a57909aa15 388
tashworth 21:0907e1f5e16c 389 slightMove(FORWARD,3250);
tashworth 16:8bb212df81b7 390 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 6:75259c3306dd 391
tashworth 16:8bb212df81b7 392 state = IDENTIFY_TOOLS;
tashworth 16:8bb212df81b7 393 break;
tashworth 12:284be46593ae 394
tashworth 13:529323807361 395
tashworth 13:529323807361 396
tashworth 16:8bb212df81b7 397 /**************************************************
tashworth 16:8bb212df81b7 398 * STAGE 3
tashworth 16:8bb212df81b7 399 *
tashworth 16:8bb212df81b7 400 * - Determine order of tools
tashworth 16:8bb212df81b7 401 * - Aquire appropriate tool
tashworth 16:8bb212df81b7 402 *
tashworth 16:8bb212df81b7 403 **************************************************/
tashworth 16:8bb212df81b7 404 case IDENTIFY_TOOLS:
tashworth 3:b7b4780a7f6e 405
tashworth 20:55dcff40c5d9 406 //wait(5);
tashworth 16:8bb212df81b7 407 servoPosition(TOOL_2); //arm/camera looks over tool
tashworth 16:8bb212df81b7 408 wait(5); //wait for servos to settle
tashworth 13:529323807361 409
tashworth 16:8bb212df81b7 410 //shape_detected = shapeDetection(); //determines the shape
tashworth 16:8bb212df81b7 411 //clearBounds();
tashworth 16:8bb212df81b7 412 //printImageToFile(BINARY);
tashworth 16:8bb212df81b7 413 shape_alignX_done = 0;
tashworth 16:8bb212df81b7 414 shape_alignY_done = 0;
tashworth 17:a5bb85ee205d 415 /*
tashworth 17:a5bb85ee205d 416 while( shape_alignY_done == 0) {
tashworth 17:a5bb85ee205d 417 wait(1);
tashworth 17:a5bb85ee205d 418 shape_detected = shapeDetection();
tashworth 17:a5bb85ee205d 419
tashworth 17:a5bb85ee205d 420 pc.printf("Y - Adjust to center tool\n\r");
tashworth 17:a5bb85ee205d 421
tashworth 17:a5bb85ee205d 422 if(get_com_y() < 50) {
tashworth 17:a5bb85ee205d 423 wait(1);
tashworth 17:a5bb85ee205d 424 slightMove(FORWARD,25);
tashworth 17:a5bb85ee205d 425 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 17:a5bb85ee205d 426
tashworth 17:a5bb85ee205d 427 } else if(get_com_y() > 70) {
tashworth 17:a5bb85ee205d 428 wait(1);
tashworth 17:a5bb85ee205d 429 slightMove(BACKWARD,25);
tashworth 17:a5bb85ee205d 430 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 17:a5bb85ee205d 431
tashworth 17:a5bb85ee205d 432 } else {
tashworth 17:a5bb85ee205d 433 shape_alignY_done = 1;
tashworth 17:a5bb85ee205d 434 }
tashworth 17:a5bb85ee205d 435 }*/
tashworth 17:a5bb85ee205d 436
tashworth 20:55dcff40c5d9 437 // aveArea = sumArea/count;
tashworth 17:a5bb85ee205d 438 //printImageToFile(BINARY);
tashworth 16:8bb212df81b7 439 //either goes to aquire the tool or look at the next shape
tashworth 20:55dcff40c5d9 440 if(Xadjust(TOOL_2) == tool_needed) {
tashworth 20:55dcff40c5d9 441 //printImageToFile(BINARY);
tashworth 16:8bb212df81b7 442 state = AQUIRE_TOOL2;
tashworth 16:8bb212df81b7 443 break;
tashworth 13:529323807361 444 } else {
tashworth 20:55dcff40c5d9 445 //printImageToFile(BINARY);
tashworth 21:0907e1f5e16c 446 slightMove(FORWARD,70);
tashworth 16:8bb212df81b7 447 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 16:8bb212df81b7 448 servoPosition(TOOL_1);
tashworth 16:8bb212df81b7 449 wait(5); //wait for servos to settle
tashworth 16:8bb212df81b7 450 shape_alignX_done = 0;
tashworth 16:8bb212df81b7 451 shape_alignY_done = 0;
tashworth 17:a5bb85ee205d 452 /*
tashworth 16:8bb212df81b7 453 while( shape_alignY_done == 0) {
tashworth 16:8bb212df81b7 454 wait(1);
tashworth 16:8bb212df81b7 455 shape_detected = shapeDetection();
tashworth 16:8bb212df81b7 456
tashworth 16:8bb212df81b7 457 pc.printf("Y - Adjust to center tool\n\r");
tashworth 16:8bb212df81b7 458
tashworth 17:a5bb85ee205d 459 if(get_com_y() < 50) {
tashworth 16:8bb212df81b7 460 wait(1);
tashworth 16:8bb212df81b7 461 slightMove(FORWARD,25);
tashworth 16:8bb212df81b7 462 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 16:8bb212df81b7 463
tashworth 17:a5bb85ee205d 464 } else if(get_com_y() > 70) {
tashworth 16:8bb212df81b7 465 wait(1);
tashworth 16:8bb212df81b7 466 slightMove(BACKWARD,25);
tashworth 16:8bb212df81b7 467 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 16:8bb212df81b7 468
tashworth 16:8bb212df81b7 469 } else {
tashworth 16:8bb212df81b7 470 shape_alignY_done = 1;
tashworth 16:8bb212df81b7 471 }
tashworth 17:a5bb85ee205d 472 }*/
tashworth 17:a5bb85ee205d 473
tashworth 20:55dcff40c5d9 474 if (Xadjust(TOOL_1) == tool_needed) {
tashworth 16:8bb212df81b7 475 state = AQUIRE_TOOL1;
tashworth 16:8bb212df81b7 476 break;
tashworth 16:8bb212df81b7 477 } else {
tashworth 16:8bb212df81b7 478 servoPosition(TOOL_3);
tashworth 16:8bb212df81b7 479 wait(3); //wait for servos to settle
tashworth 16:8bb212df81b7 480 state = AQUIRE_TOOL3;
tashworth 16:8bb212df81b7 481 }
tashworth 13:529323807361 482 }
tashworth 6:75259c3306dd 483
tashworth 16:8bb212df81b7 484 break;
tashworth 16:8bb212df81b7 485
tashworth 16:8bb212df81b7 486 case AQUIRE_TOOL1:
tashworth 0:1b64a0cedc5d 487
tashworth 16:8bb212df81b7 488 servoPosition(PU_TOOL_1);
tashworth 18:a0ea7ecaf4fe 489 setServoPulse(4, 175);
tashworth 16:8bb212df81b7 490 wait(5);
tashworth 20:55dcff40c5d9 491 setServoPulse(0, Arm_Table[PU_TOOL_1].base_rotate - 1);
tashworth 16:8bb212df81b7 492 wait(1);
tashworth 16:8bb212df81b7 493 setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 5);
tashworth 16:8bb212df81b7 494 wait(1);
tashworth 16:8bb212df81b7 495 setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 6);
tashworth 16:8bb212df81b7 496 wait(.5);
tashworth 21:0907e1f5e16c 497 setServoPulse(5, 140);
tashworth 16:8bb212df81b7 498 wait(.5);
tashworth 16:8bb212df81b7 499 setServoPulse(5, 00);
tashworth 17:a5bb85ee205d 500 setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm - 10);
tashworth 16:8bb212df81b7 501 wait(1);
tashworth 21:0907e1f5e16c 502 setServoPulse(5, 140);
tashworth 16:8bb212df81b7 503 wait(1);
tashworth 16:8bb212df81b7 504 setServoPulse(1, Arm_Table[PU_TOOL_1].base_arm);
tashworth 17:a5bb85ee205d 505 wait(2);
tashworth 14:784acd735b8c 506
tashworth 17:a5bb85ee205d 507 setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
tashworth 17:a5bb85ee205d 508 setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
tashworth 17:a5bb85ee205d 509 setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
tashworth 17:a5bb85ee205d 510 setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
tashworth 17:a5bb85ee205d 511 setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
tashworth 17:a5bb85ee205d 512 setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
tashworth 14:784acd735b8c 513
tashworth 17:a5bb85ee205d 514
tashworth 16:8bb212df81b7 515 state = NAVIGATE_WAVES_ROW1;
tashworth 16:8bb212df81b7 516 break;
tashworth 14:784acd735b8c 517
tashworth 16:8bb212df81b7 518 case AQUIRE_TOOL2:
tashworth 16:8bb212df81b7 519 servoPosition(PU_TOOL_2);
tashworth 17:a5bb85ee205d 520 setServoPulse(4, 175);
tashworth 16:8bb212df81b7 521 wait(5);
tashworth 20:55dcff40c5d9 522 setServoPulse(0, Arm_Table[PU_TOOL_2].base_rotate - 1);
tashworth 16:8bb212df81b7 523 wait(1);
tashworth 17:a5bb85ee205d 524 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
tashworth 17:a5bb85ee205d 525 wait(1);
tashworth 17:a5bb85ee205d 526 setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 2);
tashworth 16:8bb212df81b7 527 wait(1);
tashworth 16:8bb212df81b7 528 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 6);
tashworth 17:a5bb85ee205d 529 wait(2);
tashworth 21:0907e1f5e16c 530 setServoPulse(5, 140);
tashworth 17:a5bb85ee205d 531 wait(2);
tashworth 16:8bb212df81b7 532 setServoPulse(5, 00);
tashworth 17:a5bb85ee205d 533 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 10);
tashworth 17:a5bb85ee205d 534 wait(2);
tashworth 21:0907e1f5e16c 535 setServoPulse(5, 140);
tashworth 17:a5bb85ee205d 536 wait(2);
tashworth 16:8bb212df81b7 537 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm);
tashworth 17:a5bb85ee205d 538
tashworth 17:a5bb85ee205d 539 wait(2);
tashworth 0:1b64a0cedc5d 540
tashworth 17:a5bb85ee205d 541 setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
tashworth 17:a5bb85ee205d 542 setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
tashworth 17:a5bb85ee205d 543 setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
tashworth 17:a5bb85ee205d 544 setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
tashworth 17:a5bb85ee205d 545 setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
tashworth 17:a5bb85ee205d 546 setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
tashworth 17:a5bb85ee205d 547
tashworth 17:a5bb85ee205d 548
tashworth 16:8bb212df81b7 549 state = NAVIGATE_WAVES_ROW1;
tashworth 16:8bb212df81b7 550 break;
tashworth 14:784acd735b8c 551
tashworth 16:8bb212df81b7 552 case AQUIRE_TOOL3:
tashworth 17:a5bb85ee205d 553 /*
tashworth 17:a5bb85ee205d 554 while( shape_alignY_done == 0) {
tashworth 17:a5bb85ee205d 555 wait(1);
tashworth 17:a5bb85ee205d 556
tashworth 17:a5bb85ee205d 557 servoPosition(PU_TOOL_3);
tashworth 17:a5bb85ee205d 558 shape_detected = shapeDetection();
tashworth 17:a5bb85ee205d 559 wait(2);
tashworth 17:a5bb85ee205d 560
tashworth 17:a5bb85ee205d 561 pc.printf("Y - Adjust to center tool\n\r");
tashworth 17:a5bb85ee205d 562
tashworth 17:a5bb85ee205d 563 if(get_com_y() < 50) {
tashworth 17:a5bb85ee205d 564 wait(1);
tashworth 17:a5bb85ee205d 565 slightMove(FORWARD,25);
tashworth 17:a5bb85ee205d 566 current-=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 17:a5bb85ee205d 567
tashworth 17:a5bb85ee205d 568 } else if(get_com_y() > 70) {
tashworth 17:a5bb85ee205d 569 wait(1);
tashworth 17:a5bb85ee205d 570 slightMove(BACKWARD,25);
tashworth 17:a5bb85ee205d 571 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 17:a5bb85ee205d 572
tashworth 17:a5bb85ee205d 573 } else {
tashworth 17:a5bb85ee205d 574 shape_alignY_done = 1;
tashworth 17:a5bb85ee205d 575 }
tashworth 17:a5bb85ee205d 576 }
tashworth 17:a5bb85ee205d 577
tashworth 17:a5bb85ee205d 578 */
tashworth 20:55dcff40c5d9 579 Xadjust(TOOL_3);
tashworth 17:a5bb85ee205d 580
tashworth 18:a0ea7ecaf4fe 581 setServoPulse(4, 175);
tashworth 16:8bb212df81b7 582 wait(5);
tashworth 16:8bb212df81b7 583 setServoPulse(0, Arm_Table[PU_TOOL_3].base_rotate );
tashworth 16:8bb212df81b7 584 wait(1);
tashworth 17:a5bb85ee205d 585 setServoPulse(1, Arm_Table[PU_TOOL_2].base_arm - 2);
tashworth 17:a5bb85ee205d 586 wait(1);
tashworth 17:a5bb85ee205d 587 setServoPulse(3, Arm_Table[PU_TOOL_2].claw_arm - 3);
tashworth 16:8bb212df81b7 588 wait(1);
tashworth 16:8bb212df81b7 589 setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 6);
tashworth 16:8bb212df81b7 590 wait(.5);
tashworth 21:0907e1f5e16c 591 setServoPulse(5, 140);
tashworth 16:8bb212df81b7 592 wait(.5);
tashworth 16:8bb212df81b7 593 setServoPulse(5, 00);
tashworth 17:a5bb85ee205d 594 setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm - 10);
tashworth 16:8bb212df81b7 595 wait(1);
tashworth 21:0907e1f5e16c 596 setServoPulse(5, 140);
tashworth 16:8bb212df81b7 597 wait(1);
tashworth 16:8bb212df81b7 598 setServoPulse(1, Arm_Table[PU_TOOL_3].base_arm);
tashworth 17:a5bb85ee205d 599
tashworth 17:a5bb85ee205d 600 wait(2);
tashworth 14:784acd735b8c 601
tashworth 17:a5bb85ee205d 602 setServoPulse(2, Arm_Table[DRIVE_POSITION_TOOL].big_arm);
tashworth 17:a5bb85ee205d 603 setServoPulse(3, Arm_Table[DRIVE_POSITION_TOOL].claw_arm);
tashworth 17:a5bb85ee205d 604 setServoPulse(1, Arm_Table[DRIVE_POSITION_TOOL].base_arm);
tashworth 17:a5bb85ee205d 605 setServoPulse(0, Arm_Table[DRIVE_POSITION_TOOL].base_rotate);
tashworth 17:a5bb85ee205d 606 setServoPulse(4, Arm_Table[DRIVE_POSITION_TOOL].claw_rotate);
tashworth 17:a5bb85ee205d 607 setServoPulse(5, Arm_Table[DRIVE_POSITION_TOOL].claw_open);
tashworth 17:a5bb85ee205d 608
tashworth 16:8bb212df81b7 609 state = NAVIGATE_WAVES_ROW1;
tashworth 16:8bb212df81b7 610 break;
tashworth 0:1b64a0cedc5d 611
tashworth 0:1b64a0cedc5d 612
tashworth 16:8bb212df81b7 613 /**************************************************
tashworth 16:8bb212df81b7 614 * STAGE 4
tashworth 16:8bb212df81b7 615 *
tashworth 16:8bb212df81b7 616 * - Navigate through the ocean
tashworth 16:8bb212df81b7 617 *
tashworth 16:8bb212df81b7 618 **************************************************/
tashworth 0:1b64a0cedc5d 619
tashworth 16:8bb212df81b7 620 case NAVIGATE_WAVES_ROW1:
tashworth 17:a5bb85ee205d 621 from_tools_section(location,current);
tashworth 17:a5bb85ee205d 622
tashworth 17:a5bb85ee205d 623 mid_section(location, current, direction);
tashworth 17:a5bb85ee205d 624
tashworth 16:8bb212df81b7 625 state = NAVIGATE_WAVES_ROW2;
tashworth 16:8bb212df81b7 626 break;
tashworth 16:8bb212df81b7 627
tashworth 16:8bb212df81b7 628 case NAVIGATE_WAVES_ROW2:
tashworth 17:a5bb85ee205d 629 mid_section2(location, current, direction);
tashworth 16:8bb212df81b7 630 state = NAVIGATE_WAVES_ROW3;
tashworth 16:8bb212df81b7 631 break;
tashworth 16:8bb212df81b7 632
tashworth 16:8bb212df81b7 633 case NAVIGATE_WAVES_ROW3:
tashworth 3:b7b4780a7f6e 634
tashworth 16:8bb212df81b7 635 if(shape_detected == 1) {
tashworth 17:a5bb85ee205d 636 rig_section(location, current, direction, 1);
tashworth 17:a5bb85ee205d 637 while(1);
tashworth 16:8bb212df81b7 638 state = NAVIGATE_TO_SQUARE_RIG;
tashworth 16:8bb212df81b7 639 } else if(shape_detected == 2) {
tashworth 17:a5bb85ee205d 640 rig_section(location, current, direction, 2);
tashworth 17:a5bb85ee205d 641 while(1);
tashworth 16:8bb212df81b7 642 state = NAVIGATE_TO_TRIANGLE_RIG;
tashworth 16:8bb212df81b7 643 } else {
tashworth 17:a5bb85ee205d 644 rig_section(location, current, direction, 3);
tashworth 17:a5bb85ee205d 645 while(1);
tashworth 16:8bb212df81b7 646 state = NAVIGATE_TO_CIRCLE_RIG;
tashworth 16:8bb212df81b7 647 }
tashworth 16:8bb212df81b7 648 break;
tashworth 14:784acd735b8c 649
tashworth 16:8bb212df81b7 650 /**************************************************
tashworth 16:8bb212df81b7 651 * STAGE 5
tashworth 16:8bb212df81b7 652 *
tashworth 16:8bb212df81b7 653 * - Travel to appropriate rig
tashworth 16:8bb212df81b7 654 *
tashworth 16:8bb212df81b7 655 **************************************************/
tashworth 16:8bb212df81b7 656 case NAVIGATE_TO_SQUARE_RIG:
tashworth 16:8bb212df81b7 657 //NAVIGATION CODE HERE
tashworth 16:8bb212df81b7 658 state = RIG_ALIGN;
tashworth 16:8bb212df81b7 659 break;
tashworth 16:8bb212df81b7 660 case NAVIGATE_TO_TRIANGLE_RIG:
tashworth 16:8bb212df81b7 661 //NAVIGATION CODE HERE
tashworth 16:8bb212df81b7 662 state = RIG_ALIGN;
tashworth 16:8bb212df81b7 663 break;
tashworth 16:8bb212df81b7 664 case NAVIGATE_TO_CIRCLE_RIG:
tashworth 16:8bb212df81b7 665 //NAVIGATION CODE HERE
tashworth 16:8bb212df81b7 666 state = RIG_ALIGN;
tashworth 16:8bb212df81b7 667 break;
tashworth 6:75259c3306dd 668
tashworth 16:8bb212df81b7 669 /**************************************************
tashworth 16:8bb212df81b7 670 * STAGE 6
tashworth 16:8bb212df81b7 671 *
tashworth 16:8bb212df81b7 672 * - Align with appropriate rig
tashworth 16:8bb212df81b7 673 *
tashworth 16:8bb212df81b7 674 **************************************************/
tashworth 16:8bb212df81b7 675 case RIG_ALIGN:
tashworth 16:8bb212df81b7 676
tashworth 16:8bb212df81b7 677 //*********************//
tashworth 16:8bb212df81b7 678 //******* TODO ********//
tashworth 16:8bb212df81b7 679 //*********************//
tashworth 16:8bb212df81b7 680 // CODE TO ALIGN ROBOT WITH RIG
tashworth 13:529323807361 681
tashworth 16:8bb212df81b7 682 servoPosition(ORIENT_TOOL);
tashworth 16:8bb212df81b7 683 wait(1); //wait for servos to settle
tashworth 16:8bb212df81b7 684 state = INSERT_TOOL;
tashworth 16:8bb212df81b7 685 break;
tashworth 14:784acd735b8c 686
tashworth 16:8bb212df81b7 687 /**************************************************
tashworth 16:8bb212df81b7 688 * STAGE 7
tashworth 16:8bb212df81b7 689 *
tashworth 16:8bb212df81b7 690 * - Insert Tool
tashworth 16:8bb212df81b7 691 * - Extenguish fire
tashworth 16:8bb212df81b7 692 * - win contest
tashworth 16:8bb212df81b7 693 *
tashworth 16:8bb212df81b7 694 **************************************************/
tashworth 0:1b64a0cedc5d 695
tashworth 16:8bb212df81b7 696 case INSERT_TOOL:
tashworth 16:8bb212df81b7 697 //*********************//
tashworth 16:8bb212df81b7 698 //******* TODO ********//
tashworth 16:8bb212df81b7 699 //*********************//
tashworth 16:8bb212df81b7 700 // CODE TO INSERT TOOL
tashworth 16:8bb212df81b7 701 break;
tashworth 8:77a57909aa15 702
tashworth 16:8bb212df81b7 703 /**************************************************
tashworth 16:8bb212df81b7 704 * STAGE 8
tashworth 16:8bb212df81b7 705 *
tashworth 16:8bb212df81b7 706 * - END COMPETITION
tashworth 16:8bb212df81b7 707 *
tashworth 16:8bb212df81b7 708 **************************************************/
tashworth 16:8bb212df81b7 709 case END:
tashworth 16:8bb212df81b7 710 servoPosition(STORE_POSITION);
tashworth 16:8bb212df81b7 711 myled1 = 1;
tashworth 16:8bb212df81b7 712 wait(.2);
tashworth 16:8bb212df81b7 713 myled2 = 1;
tashworth 16:8bb212df81b7 714 wait(.2);
tashworth 16:8bb212df81b7 715 myled3 = 1;
tashworth 16:8bb212df81b7 716 wait(.2);
tashworth 16:8bb212df81b7 717 myled4 = 1;
tashworth 16:8bb212df81b7 718 wait(.2);
tashworth 16:8bb212df81b7 719 break;
tashworth 16:8bb212df81b7 720 default:
tashworth 13:529323807361 721
tashworth 16:8bb212df81b7 722 break;
tashworth 16:8bb212df81b7 723 }
tashworth 16:8bb212df81b7 724 } // End while loop
tashworth 16:8bb212df81b7 725
tashworth 16:8bb212df81b7 726 } // End if for start button
tashworth 14:784acd735b8c 727
tashworth 14:784acd735b8c 728
tashworth 16:8bb212df81b7 729 } // main loop
tashworth 13:529323807361 730
tashworth 13:529323807361 731
tashworth 8:77a57909aa15 732
tashworth 14:784acd735b8c 733 /************
tashworth 14:784acd735b8c 734
tashworth 14:784acd735b8c 735 Servo Functions
tashworth 14:784acd735b8c 736
tashworth 14:784acd735b8c 737 **************/
tashworth 14:784acd735b8c 738
tashworth 14:784acd735b8c 739 void setServoPulse(uint8_t n, int angle)
tashworth 14:784acd735b8c 740 {
tashworth 14:784acd735b8c 741 int pulse;
tashworth 14:784acd735b8c 742 pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 14:784acd735b8c 743 float pulselength = 20000; // 20,000 us per second
tashworth 14:784acd735b8c 744 int i = currentPosition[n];
tashworth 20:55dcff40c5d9 745 //pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]);
tashworth 14:784acd735b8c 746 int pulse2;
tashworth 14:784acd735b8c 747 if(currentPosition[n] < pulse) {
tashworth 14:784acd735b8c 748 for(i; i < pulse; i++) {
tashworth 14:784acd735b8c 749 pulse2 = 4094 * i / pulselength;
tashworth 14:784acd735b8c 750 pwm.setPWM(n, 0, pulse2);
tashworth 14:784acd735b8c 751 wait_ms(3);
tashworth 14:784acd735b8c 752 }
tashworth 14:784acd735b8c 753 } else if (currentPosition[n] > pulse) {
tashworth 14:784acd735b8c 754 for(i; i > pulse; i--) {
tashworth 14:784acd735b8c 755 pulse2 = 4094 * i / pulselength;
tashworth 14:784acd735b8c 756 pwm.setPWM(n, 0, pulse2);
tashworth 14:784acd735b8c 757 wait_ms(3);
tashworth 13:529323807361 758 }
tashworth 14:784acd735b8c 759 }
tashworth 14:784acd735b8c 760 currentPosition[n] = i;
tashworth 20:55dcff40c5d9 761 //pc.printf("END: pulse: %d, angle: %d\n\r", i, angle);
tashworth 14:784acd735b8c 762 }
tashworth 14:784acd735b8c 763
tashworth 14:784acd735b8c 764 void initServoDriver(void)
tashworth 14:784acd735b8c 765 {
tashworth 14:784acd735b8c 766 pwm.begin();
tashworth 14:784acd735b8c 767 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 14:784acd735b8c 768 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 14:784acd735b8c 769 pwm.setI2Cfreq(400000); //400kHz
tashworth 14:784acd735b8c 770
tashworth 14:784acd735b8c 771 }
tashworth 14:784acd735b8c 772
tashworth 14:784acd735b8c 773 void servoBegin(void)
tashworth 14:784acd735b8c 774 {
tashworth 14:784acd735b8c 775 pc.printf("Setting Initial Servo Position\n\r");
tashworth 16:8bb212df81b7 776 setServoPulseNo_delay(3, Arm_Table[STORE_POSITION].claw_arm);
tashworth 16:8bb212df81b7 777 setServoPulseNo_delay(2, Arm_Table[STORE_POSITION].big_arm);
tashworth 16:8bb212df81b7 778 wait(2);
tashworth 16:8bb212df81b7 779 setServoPulseNo_delay(1, Arm_Table[STORE_POSITION].base_arm);
tashworth 16:8bb212df81b7 780 setServoPulseNo_delay(0, Arm_Table[STORE_POSITION].base_rotate);
tashworth 16:8bb212df81b7 781 setServoPulseNo_delay(4, Arm_Table[STORE_POSITION].claw_rotate);
tashworth 16:8bb212df81b7 782 setServoPulseNo_delay(5, Arm_Table[STORE_POSITION].claw_open);
tashworth 14:784acd735b8c 783 }
tashworth 14:784acd735b8c 784
tashworth 14:784acd735b8c 785 void setServoPulseNo_delay(uint8_t n, int angle)
tashworth 14:784acd735b8c 786 {
tashworth 14:784acd735b8c 787 int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 14:784acd735b8c 788 float pulselength = 20000; // 20,000 us per second
tashworth 14:784acd735b8c 789 currentPosition[n] = pulse;
tashworth 14:784acd735b8c 790 //pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle);
tashworth 14:784acd735b8c 791 pulse = 4094 * pulse / pulselength;
tashworth 14:784acd735b8c 792 pwm.setPWM(n, 0, pulse);
tashworth 14:784acd735b8c 793
tashworth 14:784acd735b8c 794 }
tashworth 17:a5bb85ee205d 795 void setServoPulse2(uint8_t n, float angle)
tashworth 17:a5bb85ee205d 796 {
tashworth 17:a5bb85ee205d 797 float pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE);
tashworth 17:a5bb85ee205d 798 float pulselength = 20000; // 10,000 us per second
tashworth 17:a5bb85ee205d 799 pulse = 4094 * pulse / pulselength;
tashworth 17:a5bb85ee205d 800 pwm.setPWM(n, 0, pulse);
tashworth 17:a5bb85ee205d 801 }
tashworth 14:784acd735b8c 802
tashworth 14:784acd735b8c 803
tashworth 14:784acd735b8c 804
tashworth 14:784acd735b8c 805 void servoPosition(int set)
tashworth 14:784acd735b8c 806 {
tashworth 14:784acd735b8c 807 //moves to current position
tashworth 14:784acd735b8c 808 setServoPulse(3, Arm_Table[set].claw_arm);
tashworth 14:784acd735b8c 809 setServoPulse(2, Arm_Table[set].big_arm);
tashworth 14:784acd735b8c 810 setServoPulse(1, Arm_Table[set].base_arm);
tashworth 14:784acd735b8c 811 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 14:784acd735b8c 812 setServoPulse(4, Arm_Table[set].claw_rotate);
tashworth 14:784acd735b8c 813 setServoPulse(5, Arm_Table[set].claw_open);
tashworth 14:784acd735b8c 814 }
tashworth 8:77a57909aa15 815
tashworth 8:77a57909aa15 816
tashworth 20:55dcff40c5d9 817
tashworth 20:55dcff40c5d9 818
tashworth 20:55dcff40c5d9 819
tashworth 20:55dcff40c5d9 820
tashworth 20:55dcff40c5d9 821
tashworth 20:55dcff40c5d9 822
tashworth 14:784acd735b8c 823 int fire_checker(int rig)
tashworth 14:784acd735b8c 824 {
tashworth 14:784acd735b8c 825 switch (rig) {
tashworth 8:77a57909aa15 826
tashworth 14:784acd735b8c 827 case 1:
tashworth 14:784acd735b8c 828 for (int i = 0; i<10; i++) {
tashworth 14:784acd735b8c 829 distLaser = getDistance();
tashworth 14:784acd735b8c 830 pc.printf("L DISTANCE: %d \n\r", distLaser);
tashworth 14:784acd735b8c 831 if ((distLaser < OILRIG1_MAX)
tashworth 14:784acd735b8c 832 && (distLaser > OILRIG1_MIN)) {
tashworth 14:784acd735b8c 833 fire_detected++;
tashworth 8:77a57909aa15 834 } else {
tashworth 14:784acd735b8c 835 fire_not_detected++;
tashworth 8:77a57909aa15 836 }
tashworth 14:784acd735b8c 837 }
tashworth 14:784acd735b8c 838 break;
tashworth 14:784acd735b8c 839 case 2:
tashworth 14:784acd735b8c 840 for (int i = 0; i<10; i++) {
tashworth 14:784acd735b8c 841 distLaser = getDistance();
tashworth 14:784acd735b8c 842 pc.printf("L DISTANCE: %d \n\r", distLaser);
tashworth 14:784acd735b8c 843 if ((distLaser < OILRIG2_MAX)
tashworth 14:784acd735b8c 844 && (distLaser > OILRIG2_MIN)) {
tashworth 14:784acd735b8c 845 fire_detected++;
tashworth 13:529323807361 846 } else {
tashworth 14:784acd735b8c 847 fire_not_detected++;
tashworth 13:529323807361 848 }
tashworth 13:529323807361 849 }
tashworth 14:784acd735b8c 850 break;
tashworth 8:77a57909aa15 851
tashworth 14:784acd735b8c 852 }
tashworth 14:784acd735b8c 853
tashworth 14:784acd735b8c 854 if (fire_detected > 0) {
tashworth 14:784acd735b8c 855 return 1;
tashworth 14:784acd735b8c 856 } else {
tashworth 14:784acd735b8c 857 return 0;
tashworth 14:784acd735b8c 858 }
tashworth 14:784acd735b8c 859 }
tashworth 14:784acd735b8c 860
tashworth 14:784acd735b8c 861 void errFunction(void)
tashworth 14:784acd735b8c 862 {
tashworth 20:55dcff40c5d9 863 pc.printf("\n\nERROR: %d", motors.getError() );
tashworth 20:55dcff40c5d9 864
tashworth 14:784acd735b8c 865 }
tashworth 20:55dcff40c5d9 866
tashworth 20:55dcff40c5d9 867
tashworth 20:55dcff40c5d9 868
tashworth 20:55dcff40c5d9 869
tashworth 14:784acd735b8c 870 void wall_follow(int side, int direction, int section)
tashworth 14:784acd735b8c 871 {
tashworth 20:55dcff40c5d9 872 float location, set=4;
tashworth 14:784acd735b8c 873 int dir=1;
tashworth 14:784acd735b8c 874
tashworth 14:784acd735b8c 875 pid1.reset();
tashworth 14:784acd735b8c 876
tashworth 14:784acd735b8c 877 if(direction == BACKWARD) dir=-1;
tashworth 14:784acd735b8c 878 if(section == TOOLS)set= 10;
tashworth 14:784acd735b8c 879
tashworth 14:784acd735b8c 880 leftEncoder.reset();
tashworth 14:784acd735b8c 881 rightEncoder.reset();
tashworth 14:784acd735b8c 882
tashworth 14:784acd735b8c 883 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 14:784acd735b8c 884
tashworth 14:784acd735b8c 885 while(location< 66.5) {
tashworth 14:784acd735b8c 886 location=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 14:784acd735b8c 887
tashworth 14:784acd735b8c 888 pid1.setInputLimits(0, set);
tashworth 14:784acd735b8c 889 pid1.setOutputLimits( -MAX_SPEED, MAX_SPEED);
tashworth 14:784acd735b8c 890 pid1.setSetPoint(set);
tashworth 14:784acd735b8c 891 if(side) {
tashworth 14:784acd735b8c 892 rangeFinderLeft.startMeas();
tashworth 14:784acd735b8c 893 wait_ms(20);
tashworth 14:784acd735b8c 894 rangeFinderLeft.getMeas(range);
tashworth 14:784acd735b8c 895 } else {
tashworth 14:784acd735b8c 896 rangeFinderRight.startMeas();
tashworth 14:784acd735b8c 897 wait_ms(20);
tashworth 14:784acd735b8c 898 rangeFinderRight.getMeas(range);
tashworth 20:55dcff40c5d9 899 //pc.printf("%d\r\n",range);
tashworth 8:77a57909aa15 900 }
tashworth 8:77a57909aa15 901
tashworth 14:784acd735b8c 902 if(range > 15) {
tashworth 14:784acd735b8c 903 //pc.printf("wavegap %f\r\n",wavegap);
tashworth 14:784acd735b8c 904 // AT WAVE OPENING!!!!
tashworth 14:784acd735b8c 905 motors.setMotor1Speed(dir*0.25*127);//left
tashworth 14:784acd735b8c 906 motors.setMotor0Speed(dir*0.25*127);//right
tashworth 14:784acd735b8c 907 } else {
tashworth 13:529323807361 908
tashworth 14:784acd735b8c 909 pid1.setProcessValue(range);
tashworth 14:784acd735b8c 910 pid_return = pid1.compute();
tashworth 13:529323807361 911
tashworth 14:784acd735b8c 912 if(pid_return > 0) {
tashworth 14:784acd735b8c 913 if(side) {
tashworth 14:784acd735b8c 914 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
tashworth 14:784acd735b8c 915 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 14:784acd735b8c 916 } else {
tashworth 14:784acd735b8c 917 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
tashworth 14:784acd735b8c 918 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 14:784acd735b8c 919 }
tashworth 14:784acd735b8c 920 } else if(pid_return < 0) {
tashworth 14:784acd735b8c 921 if(side) {
tashworth 14:784acd735b8c 922 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 14:784acd735b8c 923 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
tashworth 14:784acd735b8c 924 } else {
tashworth 14:784acd735b8c 925 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 14:784acd735b8c 926 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
tashworth 14:784acd735b8c 927 }
tashworth 14:784acd735b8c 928 } else {
tashworth 14:784acd735b8c 929 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 14:784acd735b8c 930 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 14:784acd735b8c 931 }
tashworth 14:784acd735b8c 932 }
tashworth 14:784acd735b8c 933 }
tashworth 13:529323807361 934
tashworth 14:784acd735b8c 935 //STOP
tashworth 14:784acd735b8c 936 motors.setMotor0Speed(dir*-0.3*127); //right
tashworth 14:784acd735b8c 937 motors.setMotor1Speed(dir*-0.3*127); //left
tashworth 14:784acd735b8c 938 wait_ms(10);
tashworth 14:784acd735b8c 939 motors.stopBothMotors(0);
tashworth 14:784acd735b8c 940 }
tashworth 14:784acd735b8c 941
tashworth 14:784acd735b8c 942 /* MODIFIED WALL_FOLLOW FOR NAVIGATION */
tashworth 13:529323807361 943
tashworth 14:784acd735b8c 944 void wall_follow2(int side, int direction, int section, float location, int rig)
tashworth 14:784acd735b8c 945 {
tashworth 20:55dcff40c5d9 946 int dir=1, limit=80, lowlim=4;
tashworth 20:55dcff40c5d9 947 float set=9, loc=0, Rigloc=0;
tashworth 14:784acd735b8c 948 bool SeeWaveGap = false;
tashworth 14:784acd735b8c 949
tashworth 14:784acd735b8c 950 if(rig == 1) Rigloc= 16;
tashworth 14:784acd735b8c 951 else if(rig == 2) Rigloc= 45;
tashworth 14:784acd735b8c 952 else if(rig== 3) Rigloc = 70;
tashworth 8:77a57909aa15 953
tashworth 14:784acd735b8c 954 pid1.reset();
tashworth 13:529323807361 955
tashworth 14:784acd735b8c 956 if(direction == BACKWARD) {
tashworth 14:784acd735b8c 957 dir=-1;
tashworth 14:784acd735b8c 958 limit = 100;
tashworth 20:55dcff40c5d9 959 } else if(direction == FORWARD) lowlim=-20;
tashworth 14:784acd735b8c 960 if(section == TOOLS) {
tashworth 20:55dcff40c5d9 961 set= 9;
tashworth 14:784acd735b8c 962 limit = 86;
tashworth 20:55dcff40c5d9 963 } else if(section == RIGS) set = 9;
tashworth 20:55dcff40c5d9 964 else if(section == RETURN) lowlim=4;
tashworth 20:55dcff40c5d9 965 else if(section == MID2) limit =85;
tashworth 20:55dcff40c5d9 966
tashworth 20:55dcff40c5d9 967 if(location <4) limit=80;
tashworth 13:529323807361 968
tashworth 14:784acd735b8c 969 leftEncoder.reset();
tashworth 14:784acd735b8c 970 rightEncoder.reset();
tashworth 14:784acd735b8c 971
tashworth 20:55dcff40c5d9 972 pc.printf("before %f\r\n", location);
tashworth 14:784acd735b8c 973
tashworth 20:55dcff40c5d9 974 //pc.printf("dir*loc+location %f\r\n",dir*loc + location );
tashworth 20:55dcff40c5d9 975 //pc.printf("limit %d \r\n", limit);
tashworth 14:784acd735b8c 976
tashworth 14:784acd735b8c 977 while((dir*loc + location <= limit) && (dir*loc + location >= lowlim)) {
tashworth 14:784acd735b8c 978
tashworth 14:784acd735b8c 979 loc=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 20:55dcff40c5d9 980 //pc.printf("loc %f \r\n", loc);
tashworth 13:529323807361 981
tashworth 14:784acd735b8c 982 pid1.setInputLimits(0.0, set);
tashworth 20:55dcff40c5d9 983 pid1.setOutputLimits( -MAX_SPEED1, MAX_SPEED1);
tashworth 14:784acd735b8c 984 pid1.setSetPoint(set);
tashworth 14:784acd735b8c 985
tashworth 14:784acd735b8c 986 if(side) {
tashworth 14:784acd735b8c 987 rangeFinderLeft.startMeas();
tashworth 14:784acd735b8c 988 wait_ms(20);
tashworth 14:784acd735b8c 989 rangeFinderLeft.getMeas(range);
tashworth 14:784acd735b8c 990 } else {
tashworth 14:784acd735b8c 991 rangeFinderRight.startMeas();
tashworth 14:784acd735b8c 992 wait_ms(20);
tashworth 14:784acd735b8c 993 rangeFinderRight.getMeas(range);
tashworth 14:784acd735b8c 994 }
tashworth 13:529323807361 995
tashworth 14:784acd735b8c 996 if(section == RIGS) {
tashworth 14:784acd735b8c 997 rangeFinderLeft.startMeas();
tashworth 14:784acd735b8c 998 wait_ms(20);
tashworth 14:784acd735b8c 999 rangeFinderLeft.getMeas(range2);
tashworth 14:784acd735b8c 1000
tashworth 20:55dcff40c5d9 1001 if(range2< 15) {
tashworth 14:784acd735b8c 1002 if( abs(dir*loc + location - Rigloc) < 10) {
tashworth 14:784acd735b8c 1003 //STOP
tashworth 17:a5bb85ee205d 1004 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1005 break;
tashworth 12:284be46593ae 1006 }
tashworth 12:284be46593ae 1007 }
tashworth 12:284be46593ae 1008 }
tashworth 12:284be46593ae 1009
tashworth 8:77a57909aa15 1010
tashworth 14:784acd735b8c 1011 //pc.printf("wall follow 2 range %f\r\n",range);
tashworth 14:784acd735b8c 1012 //pc.printf("loc+location = %f\r\n", loc+location);
tashworth 20:55dcff40c5d9 1013 if(range > 15 ) {
tashworth 14:784acd735b8c 1014 if(section == RIGS || section == RETURN) {
tashworth 14:784acd735b8c 1015 motors.setMotor0Speed(dir*0.25*127); //right
tashworth 14:784acd735b8c 1016 motors.setMotor1Speed(dir*0.25*127); //left
tashworth 14:784acd735b8c 1017 } else {
tashworth 14:784acd735b8c 1018 if(!SeeWaveGap) {
tashworth 20:55dcff40c5d9 1019 wait_ms(40);
tashworth 14:784acd735b8c 1020 SeeWaveGap=true;
tashworth 14:784acd735b8c 1021 } else {
tashworth 14:784acd735b8c 1022 //STOP
tashworth 17:a5bb85ee205d 1023 motors.stopBothMotors(127);
tashworth 13:529323807361 1024
tashworth 20:55dcff40c5d9 1025 //pc.printf("wavegap\r\n");
tashworth 14:784acd735b8c 1026 // AT WAVE OPENING!!!!
tashworth 12:284be46593ae 1027 break;
tashworth 12:284be46593ae 1028 }
tashworth 8:77a57909aa15 1029 }
tashworth 14:784acd735b8c 1030 } else {
tashworth 14:784acd735b8c 1031 SeeWaveGap = false;
tashworth 14:784acd735b8c 1032 pid1.setProcessValue(range);
tashworth 14:784acd735b8c 1033 pid_return = pid1.compute();
tashworth 20:55dcff40c5d9 1034 pc.printf("Range: %f\n PID: %f\r\n", range, pid_return);
tashworth 13:529323807361 1035
tashworth 14:784acd735b8c 1036 if(pid_return > 0) {
tashworth 14:784acd735b8c 1037 if(side) {
tashworth 14:784acd735b8c 1038 motors.setMotor0Speed(dir*MAX_SPEED - dir*pid_return);//right
tashworth 14:784acd735b8c 1039 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 14:784acd735b8c 1040 } else {
tashworth 14:784acd735b8c 1041 motors.setMotor1Speed(dir*MAX_SPEED - dir*pid_return);//left
tashworth 14:784acd735b8c 1042 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 12:284be46593ae 1043 }
tashworth 14:784acd735b8c 1044 } else if(pid_return < 0) {
tashworth 14:784acd735b8c 1045 if(side) {
tashworth 14:784acd735b8c 1046 motors.setMotor0Speed(dir*MAX_SPEED);//right
tashworth 14:784acd735b8c 1047 motors.setMotor1Speed(dir*MAX_SPEED + dir*pid_return);//left
tashworth 14:784acd735b8c 1048 } else {
tashworth 14:784acd735b8c 1049 motors.setMotor1Speed(dir*MAX_SPEED);//left
tashworth 14:784acd735b8c 1050 motors.setMotor0Speed(dir*MAX_SPEED + dir*pid_return);//right
tashworth 14:784acd735b8c 1051 }
tashworth 14:784acd735b8c 1052 } else {
tashworth 14:784acd735b8c 1053 motors.setMotor0Speed(dir*MAX_SPEED);
tashworth 14:784acd735b8c 1054 motors.setMotor1Speed(dir*MAX_SPEED);
tashworth 14:784acd735b8c 1055 }
tashworth 14:784acd735b8c 1056 }
tashworth 14:784acd735b8c 1057 }
tashworth 13:529323807361 1058
tashworth 14:784acd735b8c 1059 //STOP
tashworth 17:a5bb85ee205d 1060 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1061 }
tashworth 0:1b64a0cedc5d 1062
tashworth 0:1b64a0cedc5d 1063
tashworth 14:784acd735b8c 1064 void alignWithWall(int section)
tashworth 14:784acd735b8c 1065 {
tashworth 14:784acd735b8c 1066 float usValue = 0;
tashworth 14:784acd735b8c 1067
tashworth 14:784acd735b8c 1068 if(section == TOOLS) {
tashworth 20:55dcff40c5d9 1069 //pc.printf("tools section align\r\n");
tashworth 14:784acd735b8c 1070 // turn at an angle
tashworth 14:784acd735b8c 1071 leftEncoder.reset();
tashworth 14:784acd735b8c 1072 rightEncoder.reset();
tashworth 14:784acd735b8c 1073 motors.setMotor0Speed(-1.2*MAX_SPEED); //right
tashworth 14:784acd735b8c 1074 motors.setMotor1Speed(0.4*MAX_SPEED); //left
tashworth 14:784acd735b8c 1075 while(rightEncoder.getPulses()>-1000);
tashworth 14:784acd735b8c 1076 motors.stopBothMotors(0);
tashworth 14:784acd735b8c 1077
tashworth 14:784acd735b8c 1078 //go backwards toward wall
tashworth 14:784acd735b8c 1079 leftEncoder.reset();
tashworth 14:784acd735b8c 1080 rightEncoder.reset();
tashworth 14:784acd735b8c 1081 motors.setMotor0Speed(-0.25*127); //right
tashworth 14:784acd735b8c 1082 motors.setMotor1Speed(-0.25*127); //left
tashworth 14:784acd735b8c 1083 while(abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300);
tashworth 14:784acd735b8c 1084 motors.stopBothMotors(0);
tashworth 14:784acd735b8c 1085
tashworth 14:784acd735b8c 1086 // turn left towards wall
tashworth 14:784acd735b8c 1087 leftEncoder.reset();
tashworth 14:784acd735b8c 1088 rightEncoder.reset();
tashworth 14:784acd735b8c 1089 motors.setMotor0Speed(MAX_SPEED); //right
tashworth 14:784acd735b8c 1090 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 14:784acd735b8c 1091 while(rightEncoder.getPulses() < 20 || abs(leftEncoder.getPulses()) < 20);
tashworth 14:784acd735b8c 1092
tashworth 20:55dcff40c5d9 1093 motors.stopBothMotors(127);
tashworth 20:55dcff40c5d9 1094 wait_ms(300);
tashworth 20:55dcff40c5d9 1095 return;
tashworth 20:55dcff40c5d9 1096 /*
tashworth 20:55dcff40c5d9 1097 rangeFinderRight.startMeas();
tashworth 20:55dcff40c5d9 1098 wait_ms(20);
tashworth 20:55dcff40c5d9 1099 rangeFinderRight.getMeas(range);
tashworth 14:784acd735b8c 1100
tashworth 20:55dcff40c5d9 1101 if(range>15){
tashworth 20:55dcff40c5d9 1102 // turning left
tashworth 20:55dcff40c5d9 1103 leftEncoder.reset();
tashworth 20:55dcff40c5d9 1104 rightEncoder.reset();
tashworth 20:55dcff40c5d9 1105 motors.setMotor0Speed(-MAX_SPEED); //right
tashworth 20:55dcff40c5d9 1106 motors.setMotor1Speed(MAX_SPEED); //left
tashworth 20:55dcff40c5d9 1107 while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50);
tashworth 20:55dcff40c5d9 1108 motors.stopBothMotors(127);
tashworth 20:55dcff40c5d9 1109 return;
tashworth 20:55dcff40c5d9 1110 }
tashworth 20:55dcff40c5d9 1111 */
tashworth 14:784acd735b8c 1112 // turning left
tashworth 20:55dcff40c5d9 1113 //motors.setMotor0Speed(0.9*MAX_SPEED); //right
tashworth 20:55dcff40c5d9 1114 //motors.setMotor1Speed(-0.9*MAX_SPEED); //left
tashworth 14:784acd735b8c 1115
tashworth 17:a5bb85ee205d 1116 } else if(section == RIGS) {
tashworth 14:784acd735b8c 1117 // check distance to wall
tashworth 14:784acd735b8c 1118 rangeFinderRight.startMeas();
tashworth 14:784acd735b8c 1119 wait_ms(20);
tashworth 14:784acd735b8c 1120 rangeFinderRight.getMeas(range);
tashworth 14:784acd735b8c 1121
tashworth 20:55dcff40c5d9 1122 if(range < 3) return;
tashworth 12:284be46593ae 1123
tashworth 14:784acd735b8c 1124 // turn at an angle
tashworth 14:784acd735b8c 1125 leftEncoder.reset();
tashworth 14:784acd735b8c 1126 rightEncoder.reset();
tashworth 14:784acd735b8c 1127 motors.setMotor1Speed(-1.2*MAX_SPEED); //left
tashworth 14:784acd735b8c 1128 motors.setMotor0Speed(0.4*MAX_SPEED); //right
tashworth 17:a5bb85ee205d 1129 while(abs(leftEncoder.getPulses())<500);
tashworth 14:784acd735b8c 1130 motors.stopBothMotors(0);
tashworth 17:a5bb85ee205d 1131 wait(2);
tashworth 14:784acd735b8c 1132
tashworth 14:784acd735b8c 1133 //go backwards toward wall
tashworth 14:784acd735b8c 1134 leftEncoder.reset();
tashworth 14:784acd735b8c 1135 rightEncoder.reset();
tashworth 14:784acd735b8c 1136 motors.setMotor0Speed(-0.25*127); //right
tashworth 14:784acd735b8c 1137 motors.setMotor1Speed(-0.25*127); //left
tashworth 17:a5bb85ee205d 1138 while(abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200);
tashworth 14:784acd735b8c 1139 motors.stopBothMotors(0);
tashworth 17:a5bb85ee205d 1140 wait(2);
tashworth 12:284be46593ae 1141
tashworth 19:d4d967a885dc 1142 // turn right towards wall
tashworth 14:784acd735b8c 1143 leftEncoder.reset();
tashworth 14:784acd735b8c 1144 rightEncoder.reset();
tashworth 14:784acd735b8c 1145 motors.setMotor0Speed(-MAX_SPEED); //right
tashworth 14:784acd735b8c 1146 motors.setMotor1Speed(MAX_SPEED); //left
tashworth 19:d4d967a885dc 1147 while(abs(rightEncoder.getPulses()) < 50 || abs(leftEncoder.getPulses()) < 50);
tashworth 19:d4d967a885dc 1148
tashworth 19:d4d967a885dc 1149 motors.stopBothMotors(127);
tashworth 19:d4d967a885dc 1150 /* wait(2);
tashworth 14:784acd735b8c 1151
tashworth 19:d4d967a885dc 1152 // turning left
tashworth 19:d4d967a885dc 1153 motors.setMotor0Speed(-0.9*MAX_SPEED); //right
tashworth 19:d4d967a885dc 1154 motors.setMotor1Speed(0.9*MAX_SPEED); //left
tashworth 19:d4d967a885dc 1155 */
tashworth 20:55dcff40c5d9 1156 } else if(section == MID2) {
tashworth 20:55dcff40c5d9 1157 leftEncoder.reset();
tashworth 20:55dcff40c5d9 1158 rightEncoder.reset();
tashworth 20:55dcff40c5d9 1159 motors.setMotor0Speed(-1.2*MAX_SPEED); //right
tashworth 20:55dcff40c5d9 1160 motors.setMotor1Speed(0.4*MAX_SPEED); //left
tashworth 20:55dcff40c5d9 1161 while(rightEncoder.getPulses()>-1000);
tashworth 20:55dcff40c5d9 1162 motors.stopBothMotors(0);
tashworth 20:55dcff40c5d9 1163
tashworth 20:55dcff40c5d9 1164 //go backwards toward wall
tashworth 20:55dcff40c5d9 1165 leftEncoder.reset();
tashworth 20:55dcff40c5d9 1166 rightEncoder.reset();
tashworth 20:55dcff40c5d9 1167 motors.setMotor0Speed(-0.25*127); //right
tashworth 20:55dcff40c5d9 1168 motors.setMotor1Speed(-0.25*127); //left
tashworth 20:55dcff40c5d9 1169 while(abs(leftEncoder.getPulses()) < 150 || abs(rightEncoder.getPulses()) < 150);
tashworth 20:55dcff40c5d9 1170
tashworth 20:55dcff40c5d9 1171 // turn left towards wall
tashworth 20:55dcff40c5d9 1172 leftEncoder.reset();
tashworth 20:55dcff40c5d9 1173 rightEncoder.reset();
tashworth 20:55dcff40c5d9 1174 motors.setMotor0Speed(0.4*127); //right
tashworth 20:55dcff40c5d9 1175 motors.setMotor1Speed(-0.4*127); //left
tashworth 20:55dcff40c5d9 1176 while(abs(rightEncoder.getPulses()) < 65 || abs(leftEncoder.getPulses()) < 65);
tashworth 20:55dcff40c5d9 1177 motors.stopBothMotors(127);
tashworth 20:55dcff40c5d9 1178
tashworth 20:55dcff40c5d9 1179 slightMove(FORWARD,100);
tashworth 20:55dcff40c5d9 1180 return;
tashworth 20:55dcff40c5d9 1181
tashworth 20:55dcff40c5d9 1182 } else { // MID
tashworth 20:55dcff40c5d9 1183 //pc.printf("in mid section align\r\n");
tashworth 14:784acd735b8c 1184 // turn right towards wall
tashworth 14:784acd735b8c 1185 rightTurn();
tashworth 14:784acd735b8c 1186 // turning left towards wall
tashworth 14:784acd735b8c 1187 motors.setMotor0Speed(0.9*MAX_SPEED); //right
tashworth 14:784acd735b8c 1188 motors.setMotor1Speed(-0.9*MAX_SPEED); //left
tashworth 20:55dcff40c5d9 1189
tashworth 14:784acd735b8c 1190 }
tashworth 14:784acd735b8c 1191
tashworth 14:784acd735b8c 1192 usValue = 0;
tashworth 20:55dcff40c5d9 1193 /* while(1) {
tashworth 20:55dcff40c5d9 1194 if(section == 10) { // CURENTLY NOT USED (WAS RIGS)
tashworth 20:55dcff40c5d9 1195 rangeFinderRight.startMeas();
tashworth 20:55dcff40c5d9 1196 wait_ms(20);
tashworth 20:55dcff40c5d9 1197 rangeFinderRight.getMeas(range);
tashworth 20:55dcff40c5d9 1198 } else {
tashworth 20:55dcff40c5d9 1199 rangeFinderLeft.startMeas();
tashworth 20:55dcff40c5d9 1200 wait_ms(20);
tashworth 20:55dcff40c5d9 1201 rangeFinderLeft.getMeas(range);
tashworth 20:55dcff40c5d9 1202 }
tashworth 20:55dcff40c5d9 1203 //pc.printf("Range %f \t OldValue %f\n\r",range, usValue);
tashworth 20:55dcff40c5d9 1204 if(range > usValue && usValue != 0 && range < 25) {
tashworth 20:55dcff40c5d9 1205 break;
tashworth 20:55dcff40c5d9 1206 } else {
tashworth 20:55dcff40c5d9 1207 usValue = range;
tashworth 20:55dcff40c5d9 1208 }
tashworth 14:784acd735b8c 1209 }
tashworth 20:55dcff40c5d9 1210 motors.stopBothMotors(0);*/
tashworth 14:784acd735b8c 1211 }
tashworth 12:284be46593ae 1212
tashworth 14:784acd735b8c 1213 void rightTurn(void)
tashworth 14:784acd735b8c 1214 {
tashworth 14:784acd735b8c 1215 motors.begin();
tashworth 14:784acd735b8c 1216 leftEncoder.reset();
tashworth 14:784acd735b8c 1217 rightEncoder.reset();
tashworth 14:784acd735b8c 1218 motors.setMotor0Speed(-0.5*127);//right
tashworth 14:784acd735b8c 1219 motors.setMotor1Speed(0.5*127);//left
tashworth 20:55dcff40c5d9 1220 while(abs(leftEncoder.getPulses())<850 || abs(rightEncoder.getPulses())<850);
tashworth 17:a5bb85ee205d 1221 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1222 }
tashworth 14:784acd735b8c 1223
tashworth 14:784acd735b8c 1224 void leftTurn(void)
tashworth 14:784acd735b8c 1225 {
tashworth 14:784acd735b8c 1226 motors.begin();
tashworth 14:784acd735b8c 1227 leftEncoder.reset();
tashworth 14:784acd735b8c 1228 rightEncoder.reset();
tashworth 14:784acd735b8c 1229 motors.setMotor0Speed(0.5*127);// right
tashworth 14:784acd735b8c 1230 motors.setMotor1Speed(-0.5*127);// left
tashworth 20:55dcff40c5d9 1231 while(abs(leftEncoder.getPulses())<1075 || rightEncoder.getPulses()<1075);
tashworth 17:a5bb85ee205d 1232 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1233 }
tashworth 14:784acd735b8c 1234
tashworth 14:784acd735b8c 1235 void slightleft(void)
tashworth 14:784acd735b8c 1236 {
tashworth 14:784acd735b8c 1237
tashworth 14:784acd735b8c 1238 leftEncoder.reset();
tashworth 14:784acd735b8c 1239 rightEncoder.reset();
tashworth 14:784acd735b8c 1240 motors.setMotor0Speed(0.5*127);// right
tashworth 14:784acd735b8c 1241 motors.setMotor1Speed(-0.5*127);// left
tashworth 20:55dcff40c5d9 1242 while(abs(leftEncoder.getPulses())<70 || rightEncoder.getPulses()<70);
tashworth 17:a5bb85ee205d 1243 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1244 }
tashworth 14:784acd735b8c 1245
tashworth 14:784acd735b8c 1246 void slightright(void)
tashworth 14:784acd735b8c 1247 {
tashworth 14:784acd735b8c 1248
tashworth 14:784acd735b8c 1249 leftEncoder.reset();
tashworth 14:784acd735b8c 1250 rightEncoder.reset();
tashworth 14:784acd735b8c 1251 motors.setMotor0Speed(-0.4*127);// right
tashworth 14:784acd735b8c 1252 motors.setMotor1Speed(0.4*127);// left
tashworth 20:55dcff40c5d9 1253 while(abs(leftEncoder.getPulses())<200 || abs(rightEncoder.getPulses())<200);
tashworth 17:a5bb85ee205d 1254 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1255 }
tashworth 14:784acd735b8c 1256
tashworth 14:784acd735b8c 1257 void slightMove(int direction, float pulses)
tashworth 14:784acd735b8c 1258 {
tashworth 14:784acd735b8c 1259 int dir=1;
tashworth 14:784acd735b8c 1260
tashworth 14:784acd735b8c 1261 if(direction == BACKWARD) dir= -1;
tashworth 14:784acd735b8c 1262
tashworth 14:784acd735b8c 1263 leftEncoder.reset();
tashworth 14:784acd735b8c 1264 rightEncoder.reset();
tashworth 14:784acd735b8c 1265 motors.setMotor0Speed(dir*0.25*127); //right
tashworth 14:784acd735b8c 1266 motors.setMotor1Speed(dir*0.25*127); //left
tashworth 14:784acd735b8c 1267 while(abs(leftEncoder.getPulses()) < pulses || abs(rightEncoder.getPulses()) < pulses);
tashworth 14:784acd735b8c 1268
tashworth 14:784acd735b8c 1269 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1270 }
tashworth 14:784acd735b8c 1271
tashworth 14:784acd735b8c 1272 void UntilWall(int dir)
tashworth 14:784acd735b8c 1273 {
tashworth 14:784acd735b8c 1274
tashworth 14:784acd735b8c 1275 if(dir == BACKWARD) dir=-1;
tashworth 14:784acd735b8c 1276
tashworth 14:784acd735b8c 1277 leftEncoder.reset();
tashworth 14:784acd735b8c 1278 rightEncoder.reset();
tashworth 14:784acd735b8c 1279 motors.setMotor0Speed(dir*0.2*127); //right
tashworth 14:784acd735b8c 1280 motors.setMotor1Speed(dir*0.2*127); //left
tashworth 14:784acd735b8c 1281
tashworth 14:784acd735b8c 1282 range = 30;
tashworth 14:784acd735b8c 1283
tashworth 14:784acd735b8c 1284 while(range > 20) {
tashworth 14:784acd735b8c 1285 rangeFinderRight.startMeas();
tashworth 14:784acd735b8c 1286 wait_ms(20);
tashworth 14:784acd735b8c 1287 rangeFinderRight.getMeas(range);
tashworth 14:784acd735b8c 1288 }
tashworth 14:784acd735b8c 1289
tashworth 17:a5bb85ee205d 1290 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1291 }
tashworth 12:284be46593ae 1292
tashworth 14:784acd735b8c 1293 void overBump(int section)
tashworth 14:784acd735b8c 1294 {
tashworth 14:784acd735b8c 1295 int preLeft=5000, preRight=5000, out=0;
tashworth 14:784acd735b8c 1296
tashworth 14:784acd735b8c 1297 motors.begin();
tashworth 14:784acd735b8c 1298 // slight backwards
tashworth 14:784acd735b8c 1299 leftEncoder.reset();
tashworth 14:784acd735b8c 1300 rightEncoder.reset();
tashworth 14:784acd735b8c 1301 motors.setMotor0Speed(-0.25*127); //right
tashworth 14:784acd735b8c 1302 motors.setMotor1Speed(-0.25*127); //left
tashworth 14:784acd735b8c 1303 while(abs(leftEncoder.getPulses()) < 50 || abs(rightEncoder.getPulses()) < 50);
tashworth 17:a5bb85ee205d 1304 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1305
tashworth 20:55dcff40c5d9 1306 //pc.printf("slight backwards\r\n");
tashworth 14:784acd735b8c 1307 wait_ms(200);
tashworth 14:784acd735b8c 1308
tashworth 17:a5bb85ee205d 1309 // Over bump
tashworth 14:784acd735b8c 1310 leftEncoder.reset();
tashworth 14:784acd735b8c 1311 rightEncoder.reset();
tashworth 14:784acd735b8c 1312 motors.setMotor0Speed(0.3*127); //right
tashworth 14:784acd735b8c 1313 motors.setMotor1Speed(0.3*127); //left
tashworth 21:0907e1f5e16c 1314 while((abs(leftEncoder.getPulses()) < 900 || abs(rightEncoder.getPulses()) < 900) /*&& preLeft!=0*/ && IR.getIRDistance() >15 );
tashworth 20:55dcff40c5d9 1315
tashworth 14:784acd735b8c 1316
tashworth 20:55dcff40c5d9 1317
tashworth 20:55dcff40c5d9 1318 //pc.printf("forward \r\n");
tashworth 14:784acd735b8c 1319 wait_ms(200);
tashworth 14:784acd735b8c 1320
tashworth 14:784acd735b8c 1321 motors.stopBothMotors(0);
tashworth 14:784acd735b8c 1322 motors.begin();
tashworth 12:284be46593ae 1323
tashworth 14:784acd735b8c 1324 preLeft=preRight=5000 ;
tashworth 14:784acd735b8c 1325 leftEncoder.reset();
tashworth 14:784acd735b8c 1326 rightEncoder.reset();
tashworth 14:784acd735b8c 1327 motors.setMotor0Speed(.25*127); //right
tashworth 14:784acd735b8c 1328 motors.setMotor1Speed(.25*127); //left
tashworth 14:784acd735b8c 1329
tashworth 14:784acd735b8c 1330 if(section == TOOLS) {
tashworth 17:a5bb85ee205d 1331 while(IR.getIRDistance() > 10 && (abs(leftEncoder.getPulses()) < 200 || abs(rightEncoder.getPulses()) < 200) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
tashworth 14:784acd735b8c 1332
tashworth 14:784acd735b8c 1333 if(IR.getIRDistance() > 38) break;
tashworth 14:784acd735b8c 1334
tashworth 14:784acd735b8c 1335 preLeft=leftEncoder.getPulses();
tashworth 14:784acd735b8c 1336 preRight=rightEncoder.getPulses();
tashworth 14:784acd735b8c 1337 wait_ms(200);
tashworth 14:784acd735b8c 1338 }
tashworth 14:784acd735b8c 1339 } else if(section == MID || section == MID2) {
tashworth 21:0907e1f5e16c 1340 if(section == MID2){
tashworth 21:0907e1f5e16c 1341 motors.setMotor0Speed(.3*127); //right
tashworth 21:0907e1f5e16c 1342 motors.setMotor1Speed(.3*127); //left
tashworth 21:0907e1f5e16c 1343 while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 300 || abs(rightEncoder.getPulses()) < 300));
tashworth 21:0907e1f5e16c 1344 }
tashworth 20:55dcff40c5d9 1345
tashworth 20:55dcff40c5d9 1346 while(IR.getIRDistance() > 15 && (abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && (leftEncoder.getPulses() != preLeft || rightEncoder.getPulses() != preRight)) {
tashworth 14:784acd735b8c 1347
tashworth 14:784acd735b8c 1348 if(IR.getIRDistance() > 38) break;
tashworth 14:784acd735b8c 1349
tashworth 14:784acd735b8c 1350 preLeft=leftEncoder.getPulses();
tashworth 14:784acd735b8c 1351 preRight=rightEncoder.getPulses();
tashworth 14:784acd735b8c 1352 wait_ms(200);
tashworth 8:77a57909aa15 1353 }
tashworth 12:284be46593ae 1354
tashworth 17:a5bb85ee205d 1355 } else {// RIGS
tashworth 17:a5bb85ee205d 1356 while(abs(leftEncoder.getPulses()) < 220 || abs(rightEncoder.getPulses()) < 220);
tashworth 12:284be46593ae 1357
tashworth 17:a5bb85ee205d 1358 // go backwards to line up with bump
tashworth 14:784acd735b8c 1359 leftEncoder.reset();
tashworth 14:784acd735b8c 1360 rightEncoder.reset();
tashworth 12:284be46593ae 1361
tashworth 14:784acd735b8c 1362 motors.setMotor0Speed(-.15*127); //right
tashworth 14:784acd735b8c 1363 motors.setMotor1Speed(-.15*127); //left
tashworth 14:784acd735b8c 1364 while((abs(leftEncoder.getPulses()) < 100 || abs(rightEncoder.getPulses()) < 100) && preLeft!=0 ) {
tashworth 14:784acd735b8c 1365 preLeft = leftEncoder.getPulses();
tashworth 14:784acd735b8c 1366 preRight = rightEncoder.getPulses();
tashworth 14:784acd735b8c 1367 wait_ms(200);
tashworth 14:784acd735b8c 1368 if(leftEncoder.getPulses() == preLeft || rightEncoder.getPulses()== preRight) preLeft=preRight=0;
tashworth 12:284be46593ae 1369 }
tashworth 17:a5bb85ee205d 1370 motors.stopBothMotors(127);
tashworth 20:55dcff40c5d9 1371 motors.begin();
tashworth 14:784acd735b8c 1372
tashworth 14:784acd735b8c 1373 return;
tashworth 14:784acd735b8c 1374 }
tashworth 12:284be46593ae 1375
tashworth 17:a5bb85ee205d 1376 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1377 wait_ms(20);
tashworth 14:784acd735b8c 1378 motors.begin();
tashworth 14:784acd735b8c 1379
tashworth 14:784acd735b8c 1380 }
tashworth 20:55dcff40c5d9 1381
tashworth 20:55dcff40c5d9 1382
tashworth 14:784acd735b8c 1383 void to_tools_section1(float* location, float &current)
tashworth 14:784acd735b8c 1384 {
tashworth 21:0907e1f5e16c 1385 slightMove(FORWARD,6850);
tashworth 14:784acd735b8c 1386 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 14:784acd735b8c 1387
tashworth 14:784acd735b8c 1388 }
tashworth 14:784acd735b8c 1389
tashworth 14:784acd735b8c 1390 void to_tools_section2(float* location, float &current)
tashworth 14:784acd735b8c 1391 {
tashworth 20:55dcff40c5d9 1392 slightMove(FORWARD,3250);
tashworth 14:784acd735b8c 1393 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 14:784acd735b8c 1394
tashworth 14:784acd735b8c 1395 }
tashworth 8:77a57909aa15 1396
tashworth 14:784acd735b8c 1397 void from_tools_section(float* location, float &current)
tashworth 14:784acd735b8c 1398 {
tashworth 14:784acd735b8c 1399
tashworth 20:55dcff40c5d9 1400 //alignWithWall(TOOLS);
tashworth 20:55dcff40c5d9 1401 //current-=4;
tashworth 20:55dcff40c5d9 1402
tashworth 20:55dcff40c5d9 1403 //slightMove(FORWARD,150);
tashworth 20:55dcff40c5d9 1404 //current+=1;
tashworth 20:55dcff40c5d9 1405 //pc.printf("align\r\n");
tashworth 20:55dcff40c5d9 1406 //wait_ms(200);
tashworth 8:77a57909aa15 1407
tashworth 14:784acd735b8c 1408 //wall_follow2(LEFT,FORWARD,MID, current);
tashworth 14:784acd735b8c 1409
tashworth 20:55dcff40c5d9 1410 slightMove(BACKWARD,400);
tashworth 20:55dcff40c5d9 1411 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 21:0907e1f5e16c 1412
tashworth 14:784acd735b8c 1413 rangeFinderLeft.startMeas();
tashworth 14:784acd735b8c 1414 wait_ms(20);
tashworth 14:784acd735b8c 1415 rangeFinderLeft.getMeas(range);
tashworth 8:77a57909aa15 1416
tashworth 20:55dcff40c5d9 1417 if(range < 15) {
tashworth 20:55dcff40c5d9 1418 wall_follow2(LEFT,BACKWARD,MID, current,0);
tashworth 20:55dcff40c5d9 1419 //pc.printf("wall follow\r\n");
tashworth 14:784acd735b8c 1420 location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1421 current= location[0];
tashworth 20:55dcff40c5d9 1422 //pc.printf("current %f \r\n",current);
tashworth 14:784acd735b8c 1423 // go backwards
tashworth 20:55dcff40c5d9 1424 //slightMove(BACKWARD,200);
tashworth 14:784acd735b8c 1425
tashworth 14:784acd735b8c 1426 wait_ms(100);
tashworth 14:784acd735b8c 1427 leftTurn();
tashworth 20:55dcff40c5d9 1428 slightleft();
tashworth 14:784acd735b8c 1429 overBump(TOOLS);
tashworth 14:784acd735b8c 1430 } else {
tashworth 20:55dcff40c5d9 1431 //pc.printf("else greater than 20\r\n");
tashworth 14:784acd735b8c 1432 location[0]= current;
tashworth 14:784acd735b8c 1433 leftTurn();
tashworth 14:784acd735b8c 1434 overBump(TOOLS);
tashworth 14:784acd735b8c 1435 }
tashworth 14:784acd735b8c 1436
tashworth 20:55dcff40c5d9 1437 //pc.printf("First Wavegap = %f\r\n",location[0]);
tashworth 14:784acd735b8c 1438
tashworth 14:784acd735b8c 1439 }
tashworth 17:a5bb85ee205d 1440 void tools_section(float* location, float &current)
tashworth 17:a5bb85ee205d 1441 {
tashworth 17:a5bb85ee205d 1442 wall_follow(LEFT,FORWARD, TOOLS);
tashworth 17:a5bb85ee205d 1443 // current position in reference to the starting position
tashworth 17:a5bb85ee205d 1444 current+=(abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2;
tashworth 17:a5bb85ee205d 1445
tashworth 17:a5bb85ee205d 1446 //////////////////////////////// determine tool
tashworth 17:a5bb85ee205d 1447 wait(2);
tashworth 17:a5bb85ee205d 1448 ///////////////////////////////////////////////////////////////////////////////////////
tashworth 17:a5bb85ee205d 1449 // Move Forward
tashworth 17:a5bb85ee205d 1450 slightMove(FORWARD, 100);
tashworth 20:55dcff40c5d9 1451 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 17:a5bb85ee205d 1452
tashworth 17:a5bb85ee205d 1453 //////////////////////////////////////////Tool aquiring
tashworth 17:a5bb85ee205d 1454 wait(2);
tashworth 17:a5bb85ee205d 1455 //////////////////////////////////////////////////////////////////// After tool is aquired
tashworth 17:a5bb85ee205d 1456
tashworth 20:55dcff40c5d9 1457 //alignWithWall(TOOLS);
tashworth 20:55dcff40c5d9 1458 //pc.printf("align\r\n");
tashworth 20:55dcff40c5d9 1459 //wait_ms(100);
tashworth 17:a5bb85ee205d 1460
tashworth 17:a5bb85ee205d 1461 //wall_follow2(LEFT,FORWARD,MID, current);
tashworth 17:a5bb85ee205d 1462 //current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 17:a5bb85ee205d 1463
tashworth 17:a5bb85ee205d 1464 rangeFinderLeft.startMeas();
tashworth 17:a5bb85ee205d 1465 wait_ms(20);
tashworth 17:a5bb85ee205d 1466 rangeFinderLeft.getMeas(range);
tashworth 17:a5bb85ee205d 1467
tashworth 17:a5bb85ee205d 1468 if(range < 20) {
tashworth 17:a5bb85ee205d 1469 wall_follow2(LEFT,BACKWARD,TOOLS, current,0);
tashworth 20:55dcff40c5d9 1470 //pc.printf("wall follow\r\n");
tashworth 17:a5bb85ee205d 1471 location[0]= current - ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 17:a5bb85ee205d 1472 current= location[0];
tashworth 20:55dcff40c5d9 1473 //pc.printf("current %f \r\n",current);
tashworth 17:a5bb85ee205d 1474 // go backwards
tashworth 17:a5bb85ee205d 1475 leftEncoder.reset();
tashworth 17:a5bb85ee205d 1476 rightEncoder.reset();
tashworth 17:a5bb85ee205d 1477 motors.setMotor0Speed(-MAX_SPEED); //right
tashworth 17:a5bb85ee205d 1478 motors.setMotor1Speed(-MAX_SPEED); //left
tashworth 17:a5bb85ee205d 1479 while(abs(leftEncoder.getPulses()) < 120 || abs(rightEncoder.getPulses())< 120);
tashworth 17:a5bb85ee205d 1480 // hard stop
tashworth 17:a5bb85ee205d 1481
tashworth 17:a5bb85ee205d 1482 motors.stopBothMotors(127);
tashworth 17:a5bb85ee205d 1483
tashworth 17:a5bb85ee205d 1484 wait_ms(100);
tashworth 17:a5bb85ee205d 1485 leftTurn();
tashworth 17:a5bb85ee205d 1486 overBump(TOOLS);
tashworth 17:a5bb85ee205d 1487 } else {
tashworth 20:55dcff40c5d9 1488 //pc.printf("else greater than 20\r\n");
tashworth 17:a5bb85ee205d 1489 location[0]= current;
tashworth 17:a5bb85ee205d 1490 leftTurn();
tashworth 17:a5bb85ee205d 1491 overBump(TOOLS);
tashworth 17:a5bb85ee205d 1492 }
tashworth 17:a5bb85ee205d 1493
tashworth 20:55dcff40c5d9 1494 //pc.printf("First Wavegap = %f\r\n",location[0]);
tashworth 17:a5bb85ee205d 1495 }
tashworth 12:284be46593ae 1496
tashworth 14:784acd735b8c 1497 void mid_section(float* location, float &current, int* direction)
tashworth 14:784acd735b8c 1498 {
tashworth 14:784acd735b8c 1499 if(IR.getIRDistance() > 38) {
tashworth 14:784acd735b8c 1500 direction[0]= STRAIGHT;
tashworth 14:784acd735b8c 1501 overBump(MID);
tashworth 14:784acd735b8c 1502 return;
tashworth 14:784acd735b8c 1503 }
tashworth 20:55dcff40c5d9 1504 //pc.printf("before align with wall \r\n");
tashworth 20:55dcff40c5d9 1505 //alignWithWall(MID);
tashworth 20:55dcff40c5d9 1506 //current-=4;
tashworth 20:55dcff40c5d9 1507 //wait_ms(200);
tashworth 14:784acd735b8c 1508
tashworth 20:55dcff40c5d9 1509 //if(current > 20){
tashworth 20:55dcff40c5d9 1510 //alignWithWall(MID2);
tashworth 20:55dcff40c5d9 1511 //current-=4;
tashworth 20:55dcff40c5d9 1512 //}
tashworth 20:55dcff40c5d9 1513 rightTurn();
tashworth 21:0907e1f5e16c 1514 leftEncoder.reset();
tashworth 21:0907e1f5e16c 1515 rightEncoder.reset();
tashworth 21:0907e1f5e16c 1516 motors.setMotor0Speed(-0.4*127);// right
tashworth 21:0907e1f5e16c 1517 motors.setMotor1Speed(0.4*127);// left
tashworth 21:0907e1f5e16c 1518 while(abs(leftEncoder.getPulses())<50 || abs(rightEncoder.getPulses())<50);
tashworth 21:0907e1f5e16c 1519 motors.stopBothMotors(127);
tashworth 20:55dcff40c5d9 1520 //pc.printf("mid section current = %f\r\n",current);
tashworth 14:784acd735b8c 1521 wall_follow2(LEFT,FORWARD,MID, current,0);
tashworth 14:784acd735b8c 1522 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 20:55dcff40c5d9 1523 //pc.printf("after wf2 current = %f\r\n",current);
tashworth 8:77a57909aa15 1524
tashworth 14:784acd735b8c 1525 wait_ms(500);
tashworth 14:784acd735b8c 1526 rangeFinderLeft.startMeas();
tashworth 14:784acd735b8c 1527 wait_ms(20);
tashworth 14:784acd735b8c 1528 rangeFinderLeft.getMeas(range);
tashworth 14:784acd735b8c 1529
tashworth 14:784acd735b8c 1530 if(range > 20 ) {
tashworth 14:784acd735b8c 1531 direction[0]= RIGHT;
tashworth 14:784acd735b8c 1532 location[1]= current;
tashworth 20:55dcff40c5d9 1533 wait_ms(300);
tashworth 20:55dcff40c5d9 1534 slightMove(FORWARD,100);
tashworth 20:55dcff40c5d9 1535 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1536 } else {
tashworth 14:784acd735b8c 1537 direction[0]= LEFT;
tashworth 14:784acd735b8c 1538 wall_follow2(LEFT,BACKWARD,MID,current,0);
tashworth 14:784acd735b8c 1539 location[1]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1540 current= location[1];
tashworth 14:784acd735b8c 1541
tashworth 14:784acd735b8c 1542 if(location[1] < 18) {
tashworth 20:55dcff40c5d9 1543 slightMove(FORWARD, 75);
tashworth 14:784acd735b8c 1544 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 12:284be46593ae 1545 }
tashworth 20:55dcff40c5d9 1546 //slightMove(BACKWARD,100);
tashworth 12:284be46593ae 1547
tashworth 14:784acd735b8c 1548 }
tashworth 14:784acd735b8c 1549
tashworth 20:55dcff40c5d9 1550 wait_ms(200);
tashworth 20:55dcff40c5d9 1551 //pc.printf("wavegap2 = %f\r\n",location[1]);
tashworth 20:55dcff40c5d9 1552 //left turn
tashworth 20:55dcff40c5d9 1553 motors.begin();
tashworth 20:55dcff40c5d9 1554 leftEncoder.reset();
tashworth 20:55dcff40c5d9 1555 rightEncoder.reset();
tashworth 20:55dcff40c5d9 1556 motors.setMotor0Speed(0.5*127);// right
tashworth 20:55dcff40c5d9 1557 motors.setMotor1Speed(-0.5*127);// left
tashworth 20:55dcff40c5d9 1558 while(abs(leftEncoder.getPulses())<1045 || rightEncoder.getPulses()<1045);
tashworth 20:55dcff40c5d9 1559 motors.stopBothMotors(127);
tashworth 14:784acd735b8c 1560
tashworth 14:784acd735b8c 1561 wait_ms(100);
tashworth 14:784acd735b8c 1562
tashworth 14:784acd735b8c 1563 overBump(MID);
tashworth 14:784acd735b8c 1564
tashworth 14:784acd735b8c 1565 }
tashworth 14:784acd735b8c 1566
tashworth 14:784acd735b8c 1567 void mid_section2(float* location, float &current, int* direction)
tashworth 14:784acd735b8c 1568 {
tashworth 20:55dcff40c5d9 1569 //pc.printf("mid section 2\r\n");
tashworth 14:784acd735b8c 1570
tashworth 14:784acd735b8c 1571 if(IR.getIRDistance() > 38) {
tashworth 14:784acd735b8c 1572 direction[1]= STRAIGHT;
tashworth 14:784acd735b8c 1573 overBump(RIGS);
tashworth 14:784acd735b8c 1574 return;
tashworth 14:784acd735b8c 1575 }
tashworth 8:77a57909aa15 1576
tashworth 20:55dcff40c5d9 1577 //alignWithWall(MID);
tashworth 20:55dcff40c5d9 1578 wait_ms(100);
tashworth 20:55dcff40c5d9 1579
tashworth 20:55dcff40c5d9 1580 rightTurn();
tashworth 20:55dcff40c5d9 1581 slightright();
tashworth 20:55dcff40c5d9 1582 wait_ms(100);
tashworth 20:55dcff40c5d9 1583
tashworth 14:784acd735b8c 1584
tashworth 14:784acd735b8c 1585 wall_follow2(LEFT,FORWARD,MID, current,0);
tashworth 14:784acd735b8c 1586 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1587
tashworth 14:784acd735b8c 1588 wait_ms(500);
tashworth 8:77a57909aa15 1589
tashworth 20:55dcff40c5d9 1590 //pc.printf("midseection 2 after wf2 %f",current);
tashworth 14:784acd735b8c 1591 rangeFinderLeft.startMeas();
tashworth 14:784acd735b8c 1592 wait_ms(20);
tashworth 14:784acd735b8c 1593 rangeFinderLeft.getMeas(range);
tashworth 12:284be46593ae 1594
tashworth 14:784acd735b8c 1595 if(range > 20 ) {
tashworth 14:784acd735b8c 1596 direction[1]= RIGHT;
tashworth 14:784acd735b8c 1597 location[2]= current;
tashworth 21:0907e1f5e16c 1598 slightMove(FORWARD,100);
tashworth 17:a5bb85ee205d 1599 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1600 } else {
tashworth 14:784acd735b8c 1601 direction[1]= LEFT;
tashworth 14:784acd735b8c 1602 wall_follow2(LEFT,BACKWARD,MID,current,0);
tashworth 14:784acd735b8c 1603 location[2]= current- ((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1604 current=location[2];
tashworth 20:55dcff40c5d9 1605 //slightMove(BACKWARD,100);
tashworth 14:784acd735b8c 1606 }
tashworth 8:77a57909aa15 1607
tashworth 20:55dcff40c5d9 1608 //LEFT turn
tashworth 20:55dcff40c5d9 1609 motors.begin();
tashworth 20:55dcff40c5d9 1610 leftEncoder.reset();
tashworth 20:55dcff40c5d9 1611 rightEncoder.reset();
tashworth 20:55dcff40c5d9 1612 motors.setMotor0Speed(0.5*127);// right
tashworth 20:55dcff40c5d9 1613 motors.setMotor1Speed(-0.5*127);// left
tashworth 20:55dcff40c5d9 1614 while(abs(leftEncoder.getPulses())<950 || rightEncoder.getPulses()<950);
tashworth 20:55dcff40c5d9 1615 motors.stopBothMotors(127);
tashworth 20:55dcff40c5d9 1616
tashworth 14:784acd735b8c 1617 overBump(RIGS);
tashworth 20:55dcff40c5d9 1618 //pc.printf("overbump rigs\r\n");
tashworth 14:784acd735b8c 1619 }
tashworth 14:784acd735b8c 1620
tashworth 14:784acd735b8c 1621 void rig_section(float* location, float &current, int* direction, int rig)
tashworth 14:784acd735b8c 1622 {
tashworth 14:784acd735b8c 1623 float loc;
tashworth 14:784acd735b8c 1624
tashworth 14:784acd735b8c 1625 if(rig == 1) loc= 15;
tashworth 14:784acd735b8c 1626 else if(rig == 2) loc= 45;
tashworth 14:784acd735b8c 1627 else loc = 75;
tashworth 14:784acd735b8c 1628
tashworth 17:a5bb85ee205d 1629 // Slight forward for turn
tashworth 20:55dcff40c5d9 1630 slightMove(FORWARD,150);
tashworth 20:55dcff40c5d9 1631 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 17:a5bb85ee205d 1632 wait_ms(100);
tashworth 14:784acd735b8c 1633 rightTurn();
tashworth 20:55dcff40c5d9 1634 //slightright();
tashworth 20:55dcff40c5d9 1635
tashworth 12:284be46593ae 1636
tashworth 14:784acd735b8c 1637 if(current > loc) {
tashworth 20:55dcff40c5d9 1638 //pc.printf("RIG section %f\r\n",current);
tashworth 14:784acd735b8c 1639 wall_follow2(RIGHT, BACKWARD, RIGS, current, rig);
tashworth 14:784acd735b8c 1640 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1641 } else {
tashworth 20:55dcff40c5d9 1642 //pc.printf("RIG section %f\r\n",current);
tashworth 14:784acd735b8c 1643 wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
tashworth 14:784acd735b8c 1644 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1645 }
tashworth 17:a5bb85ee205d 1646
tashworth 20:55dcff40c5d9 1647 alignWithWall(MID2);
tashworth 20:55dcff40c5d9 1648 current-=4;
tashworth 19:d4d967a885dc 1649 wall_follow2(RIGHT, FORWARD, RIGS, current, rig);
tashworth 20:55dcff40c5d9 1650 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 20:55dcff40c5d9 1651 slightMove(FORWARD, 75);
tashworth 19:d4d967a885dc 1652 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 20:55dcff40c5d9 1653
tashworth 20:55dcff40c5d9 1654
tashworth 20:55dcff40c5d9 1655
tashworth 20:55dcff40c5d9 1656
tashworth 20:55dcff40c5d9 1657
tashworth 14:784acd735b8c 1658 }
tashworth 14:784acd735b8c 1659
tashworth 14:784acd735b8c 1660 void tools_section_return(float* location, float &current)
tashworth 14:784acd735b8c 1661 {
tashworth 14:784acd735b8c 1662 if(location[0] > 16) {
tashworth 14:784acd735b8c 1663 leftTurn();
tashworth 14:784acd735b8c 1664 wall_follow2(LEFT, BACKWARD, RETURN, location[0], 0);
tashworth 14:784acd735b8c 1665 }
tashworth 14:784acd735b8c 1666 motors.stopBothMotors(0);
tashworth 14:784acd735b8c 1667
tashworth 14:784acd735b8c 1668 }
tashworth 8:77a57909aa15 1669
tashworth 14:784acd735b8c 1670 void mid_section_return(float* location, float &current, int* direction)
tashworth 14:784acd735b8c 1671 {
tashworth 14:784acd735b8c 1672 if(direction[0] == RIGHT) {
tashworth 14:784acd735b8c 1673 leftTurn();
tashworth 14:784acd735b8c 1674 alignWithWall(MID);
tashworth 14:784acd735b8c 1675 wall_follow2(LEFT, BACKWARD, MID, current,0);
tashworth 14:784acd735b8c 1676 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1677 rightTurn();
tashworth 14:784acd735b8c 1678 } else if(direction[0] == LEFT) {
tashworth 14:784acd735b8c 1679 leftTurn();
tashworth 14:784acd735b8c 1680 wall_follow2(RIGHT, FORWARD, MID, current,0);
tashworth 14:784acd735b8c 1681 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1682 rightTurn();
tashworth 14:784acd735b8c 1683 }
tashworth 14:784acd735b8c 1684 //ELSE and GO FORWARD
tashworth 14:784acd735b8c 1685 overBump(RIGS);
tashworth 14:784acd735b8c 1686 }
tashworth 12:284be46593ae 1687
tashworth 14:784acd735b8c 1688 void mid_section2_return(float* location, float &current, int* direction)
tashworth 14:784acd735b8c 1689 {
tashworth 14:784acd735b8c 1690 if(direction[1] == RIGHT) {
tashworth 14:784acd735b8c 1691 leftTurn();
tashworth 14:784acd735b8c 1692 wall_follow2(LEFT, BACKWARD, MID, current,0);
tashworth 14:784acd735b8c 1693 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1694 rightTurn();
tashworth 14:784acd735b8c 1695 } else if(direction[1] == LEFT) {
tashworth 14:784acd735b8c 1696 leftTurn();
tashworth 14:784acd735b8c 1697 wall_follow2(RIGHT, FORWARD, MID, current,0);
tashworth 14:784acd735b8c 1698 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1699 rightTurn();
tashworth 14:784acd735b8c 1700 }
tashworth 14:784acd735b8c 1701 //ELSE and GO FORWARD
tashworth 14:784acd735b8c 1702 overBump(MID);
tashworth 14:784acd735b8c 1703 }
tashworth 14:784acd735b8c 1704
tashworth 14:784acd735b8c 1705 void rig_section_return(float* location, float &current, int* direction)
tashworth 14:784acd735b8c 1706 {
tashworth 14:784acd735b8c 1707 if(location[2] > current) {
tashworth 14:784acd735b8c 1708 wall_follow2(RIGHT, FORWARD, MID, current,0);
tashworth 14:784acd735b8c 1709 current+=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1710 } else {
tashworth 14:784acd735b8c 1711 wall_follow2(RIGHT, BACKWARD, MID, current,0);
tashworth 14:784acd735b8c 1712 current-=((abs(leftEncoder.getPulses()*11.12/PPR) + abs(rightEncoder.getPulses()*11.12/PPR))/2);
tashworth 14:784acd735b8c 1713 }
tashworth 14:784acd735b8c 1714 rightTurn();
tashworth 14:784acd735b8c 1715 overBump(MID2);
tashworth 20:55dcff40c5d9 1716 }
tashworth 20:55dcff40c5d9 1717
tashworth 20:55dcff40c5d9 1718
tashworth 20:55dcff40c5d9 1719
tashworth 20:55dcff40c5d9 1720
tashworth 20:55dcff40c5d9 1721 int Xadjust(int tool)
tashworth 20:55dcff40c5d9 1722 {
tashworth 20:55dcff40c5d9 1723 int areaArray[10];
tashworth 20:55dcff40c5d9 1724 float C, T, S;
tashworth 20:55dcff40c5d9 1725 for(int i = 0; i < 10; i++) {
tashworth 20:55dcff40c5d9 1726 areaArray[i] = shapeDetection();
tashworth 20:55dcff40c5d9 1727 wait(2);
tashworth 20:55dcff40c5d9 1728 if(get_com_x() > X_CENTER ) {
tashworth 20:55dcff40c5d9 1729 deltaX = get_com_x()-X_CENTER;
tashworth 20:55dcff40c5d9 1730 setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
tashworth 20:55dcff40c5d9 1731 Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
tashworth 20:55dcff40c5d9 1732 }
tashworth 20:55dcff40c5d9 1733 if(get_com_x() < X_CENTER) {
tashworth 20:55dcff40c5d9 1734 deltaX = get_com_x()-X_CENTER;
tashworth 20:55dcff40c5d9 1735 setServoPulse2(0,Arm_Table[tool].base_rotate += (deltaX/60.0) );
tashworth 20:55dcff40c5d9 1736 Arm_Table[PU_TOOL_2].base_rotate = Arm_Table[TOOL_2].base_rotate;
tashworth 20:55dcff40c5d9 1737 }
tashworth 20:55dcff40c5d9 1738
tashworth 20:55dcff40c5d9 1739
tashworth 20:55dcff40c5d9 1740 }
tashworth 20:55dcff40c5d9 1741
tashworth 20:55dcff40c5d9 1742 C = normd(areaArray, 10, CIRCLE_AREA);
tashworth 21:0907e1f5e16c 1743 // S = normd(areaArray, 10, SQUARE_AREA);
tashworth 21:0907e1f5e16c 1744 // T = normd(areaArray, 10, TRIANGLE_AREA);
tashworth 21:0907e1f5e16c 1745
tashworth 20:55dcff40c5d9 1746 if((C < SQUARE_AREA) && (C > CIRCLE_AREA)) {
tashworth 20:55dcff40c5d9 1747 pc.printf("\n\nSQUARE DETECTED\n\r");
tashworth 20:55dcff40c5d9 1748 return SQUARE;
tashworth 21:0907e1f5e16c 1749 } else if( ( C > SQUARE_AREA) ) {
tashworth 21:0907e1f5e16c 1750 pc.printf("\n\nCIRCLE DETECTED\n\r");
tashworth 21:0907e1f5e16c 1751 return CIRCLE;
tashworth 20:55dcff40c5d9 1752 } else {
tashworth 20:55dcff40c5d9 1753 pc.printf("\n\nTRIANGLE DETECTED\n\r");
tashworth 20:55dcff40c5d9 1754 return TRIANGLE;
tashworth 21:0907e1f5e16c 1755 }
tashworth 21:0907e1f5e16c 1756
tashworth 21:0907e1f5e16c 1757
tashworth 21:0907e1f5e16c 1758 /*
tashworth 21:0907e1f5e16c 1759 if((C < S) && (C < T)) {
tashworth 21:0907e1f5e16c 1760 pc.printf("\n\nCIRCLE DETECTED\n\r");
tashworth 21:0907e1f5e16c 1761 return CIRCLE;
tashworth 21:0907e1f5e16c 1762 } else if( ( S<C ) && ( S<T ) ) {
tashworth 21:0907e1f5e16c 1763 pc.printf("\n\nSQUARE DETECTED\n\r");
tashworth 21:0907e1f5e16c 1764 return SQUARE;
tashworth 21:0907e1f5e16c 1765 } else {
tashworth 21:0907e1f5e16c 1766 pc.printf("\n\nTRIANGLE DETECTED\n\r");
tashworth 21:0907e1f5e16c 1767 return TRIANGLE;
tashworth 21:0907e1f5e16c 1768 }*/
tashworth 20:55dcff40c5d9 1769 }
tashworth 20:55dcff40c5d9 1770
tashworth 20:55dcff40c5d9 1771 float normd(int* pop, int count, int threshold)
tashworth 20:55dcff40c5d9 1772 {
tashworth 20:55dcff40c5d9 1773 int i = 0;
tashworth 20:55dcff40c5d9 1774 float mean=0, std=0;
tashworth 20:55dcff40c5d9 1775 for(i=0; i<count; i++) {
tashworth 20:55dcff40c5d9 1776 mean += pop[i];
tashworth 20:55dcff40c5d9 1777 }
tashworth 20:55dcff40c5d9 1778 mean /= (float)count;
tashworth 20:55dcff40c5d9 1779 pc.printf("\n\nMean: %f\n\r", mean);
tashworth 20:55dcff40c5d9 1780
tashworth 20:55dcff40c5d9 1781 for(i=0; i<count; i++) {
tashworth 20:55dcff40c5d9 1782 std += pow(((float)pop[i]-mean),2);
tashworth 20:55dcff40c5d9 1783 }
tashworth 20:55dcff40c5d9 1784 std /= (float)count;
tashworth 20:55dcff40c5d9 1785 std = sqrt(std);
tashworth 20:55dcff40c5d9 1786 //pc.printf("\n\nStd: %f\n\r", std);
tashworth 20:55dcff40c5d9 1787
tashworth 20:55dcff40c5d9 1788 //pc.printf("\n\nNorm: %f\n\r", (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2))));
tashworth 20:55dcff40c5d9 1789
tashworth 20:55dcff40c5d9 1790
tashworth 20:55dcff40c5d9 1791 //return abs(mean - threshold);
tashworth 20:55dcff40c5d9 1792 return mean;
tashworth 20:55dcff40c5d9 1793 //return (1/(std*sqrt(2*PI)))*exp(-pow(((float)threshold-mean),2)/(2*pow(std,2)));
tashworth 20:55dcff40c5d9 1794
tashworth 20:55dcff40c5d9 1795 }