Thomas Ashworth / theRobot

For IEEE Robotics

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

main.cpp@20:55dcff40c5d9, 2014-04-05 (annotated)

Committer:
tashworth
Date:
Sat Apr 05 07:26:15 2014 +0000
Revision:
20:55dcff40c5d9
Parent:
19:d4d967a885dc
Child:
21:0907e1f5e16c
4-5-14

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