Thomas Ashworth / theRobot

For IEEE Robotics

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

main.cpp@14:784acd735b8c, 2014-04-02 (annotated)

Committer:
tashworth
Date:
Wed Apr 02 03:36:49 2014 +0000
Revision:
14:784acd735b8c
Parent:
13:529323807361
Child:
16:8bb212df81b7
4-1-14 10:40pm

Who changed what in which revision?

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