Paolina Povolotskaya / theRobotNEW

ksdflsjdfljas

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

Fork of theRobot by Thomas Ashworth

main.cpp@19:9329e9be4c66, 2014-04-03 (annotated)

Committer:
Fairy_Paolina
Date:
Thu Apr 03 23:01:50 2014 +0000
Revision:
19:9329e9be4c66
Parent:
18:a0ea7ecaf4fe
Child:
20:b9cbaf7566e9
ksdhfkshadf;

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