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@30:db07aea6d119, 2014-04-17 (annotated)

Committer:
Fairy_Paolina
Date:
Thu Apr 17 20:34:17 2014 +0000
Revision:
30:db07aea6d119
Parent:
29:22b243e288c8
Child:
31:99cf9c25b0f2
PID changed ;

Who changed what in which revision?

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