Thomas Ashworth / theRobot

For IEEE Robotics

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

main.cpp@19:d4d967a885dc, 2014-04-04 (annotated)

Committer:
tashworth
Date:
Fri Apr 04 01:09:49 2014 +0000
Revision:
19:d4d967a885dc
Parent:
18:a0ea7ecaf4fe
Child:
20:55dcff40c5d9
done!!

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