Thomas Ashworth / theRobot

For IEEE Robotics

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

main.cpp@22:79c5871543b5, 2014-04-11 (annotated)

Committer:
tashworth
Date:
Fri Apr 11 21:56:18 2014 +0000
Revision:
22:79c5871543b5
Parent:
21:0907e1f5e16c 
Fixed to pick up 3rd tool

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