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@28:2bb6b0fe39d0, 2014-04-16 (annotated)

Committer:
tashworth
Date:
Wed Apr 16 20:45:30 2014 +0000
Revision:
28:2bb6b0fe39d0
Parent:
27:5540aa3c828c
Child:
29:22b243e288c8
4-16-14  ; All servo positions are calibrated; including the inserting tool into rig position

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