UH Robotics / theRobot3

uyvug

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

Fork of theRobot by Thomas Ashworth

main.cpp@27:5540aa3c828c, 2014-04-15 (annotated)

Committer:
Fairy_Paolina
Date:
Tue Apr 15 17:13:35 2014 +0000
Revision:
27:5540aa3c828c
Parent:
26:7257bd16bc67
Child:
28:2bb6b0fe39d0
almost finished, turn correction, overbump correction; ;

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