UH Robotics / theRobot3

uyvug

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

Fork of theRobot by Thomas Ashworth

main.cpp@34:cd4980573159, 2014-04-24 (annotated)

Committer:
Fairy_Paolina
Date:
Thu Apr 24 23:20:43 2014 +0000
Revision:
34:cd4980573159
Parent:
33:a41981e18a7d 
Rolls up to the rig markings. ;

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