Paolina Povolotskaya / theRobotNEW

ksdflsjdfljas

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

Fork of theRobot by Thomas Ashworth

main.cpp@21:2936d9566213, 2014-04-05 (annotated)

Committer:
Fairy_Paolina
Date:
Sat Apr 05 03:27:16 2014 +0000
Revision:
21:2936d9566213
Parent:
20:b9cbaf7566e9 
klasdjflj;adlj;fdjlkfdskjlfsdak;ljfasdjkl; ;

Who changed what in which revision?

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