Thomas Ashworth / theRobot

For IEEE Robotics

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

main.cpp@11:8d2455e383ce, 2014-04-01 (annotated)

Committer:
tashworth
Date:
Tue Apr 01 02:00:01 2014 +0000
Revision:
11:8d2455e383ce
Parent:
10:1a1d52207f59
Child:
12:284be46593ae
3-31-14 end of day

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