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@24:3369d51f6cbd, 2014-04-12 (annotated)

Committer:
Fairy_Paolina
Date:
Sat Apr 12 23:22:33 2014 +0000
Revision:
24:3369d51f6cbd
Parent:
23:f8e806d1ffcc
Child:
26:7257bd16bc67
revised;

Who changed what in which revision?

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