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@29:22b243e288c8, 2014-04-16 (annotated)

Committer:
tashworth
Date:
Wed Apr 16 20:51:52 2014 +0000
Revision:
29:22b243e288c8
Parent:
28:2bb6b0fe39d0
Child:
30:db07aea6d119
4-16-14; 4pm; All servo positions re-calibrated... including the inserting the tool position

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