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@31:99cf9c25b0f2, 2014-04-21 (annotated)

Committer:
srsmitherman
Date:
Mon Apr 21 18:27:12 2014 +0000
Revision:
31:99cf9c25b0f2
Parent:
30:db07aea6d119
Child:
32:4a42f61f64a6
Revisions to Navigation - Wave Gap Alignment

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