uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
main.cpp@5:429e9a998bab, 2014-03-14 (annotated)
- Committer:
- tashworth
- Date:
- Fri Mar 14 22:15:58 2014 +0000
- Revision:
- 5:429e9a998bab
- Parent:
- 4:42460f387701
- Child:
- 6:75259c3306dd
3-14-14
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:1b64a0cedc5d | 4 | |
tashworth | 0:1b64a0cedc5d | 5 | Serial pc(USBTX,USBRX); |
tashworth | 3:b7b4780a7f6e | 6 | Adafruit_PWMServoDriver pwm(p28,p27); |
tashworth | 0:1b64a0cedc5d | 7 | DigitalOut ServoOutputDisable(p8); |
tashworth | 0:1b64a0cedc5d | 8 | extern Serial lrf; |
tashworth | 0:1b64a0cedc5d | 9 | |
tashworth | 0:1b64a0cedc5d | 10 | //Servo Positions |
tashworth | 0:1b64a0cedc5d | 11 | #define STORE_POSITION 0 |
tashworth | 0:1b64a0cedc5d | 12 | #define OIL_RIG1 1 |
tashworth | 0:1b64a0cedc5d | 13 | #define OIL_RIG2 2 |
tashworth | 2:4e082e4c255d | 14 | #define OIL_RIG3 3 |
tashworth | 2:4e082e4c255d | 15 | #define IMG_SHAPE_1 4 |
tashworth | 2:4e082e4c255d | 16 | #define IMG_SHAPE_2 5 |
tashworth | 2:4e082e4c255d | 17 | #define IMG_SHAPE_3 6 |
tashworth | 2:4e082e4c255d | 18 | #define GRASP_SHAPE_1 7 |
tashworth | 2:4e082e4c255d | 19 | #define GRASP_SHAPE_2 8 |
tashworth | 2:4e082e4c255d | 20 | #define GRASP_SHAPE_3 9 |
tashworth | 2:4e082e4c255d | 21 | #define INSERT_TOOL_1 10 |
tashworth | 2:4e082e4c255d | 22 | #define INSERT_TOOL_2 11 |
tashworth | 2:4e082e4c255d | 23 | #define INSERT_TOOL_3 11 |
tashworth | 2:4e082e4c255d | 24 | |
tashworth | 3:b7b4780a7f6e | 25 | #define MIN_SERVO_PULSE 900 |
tashworth | 3:b7b4780a7f6e | 26 | #define MAX_SERVO_PULSE 2100 |
tashworth | 3:b7b4780a7f6e | 27 | #define SERVO_MAX_ANGLE 180 |
tashworth | 3:b7b4780a7f6e | 28 | |
tashworth | 0:1b64a0cedc5d | 29 | |
tashworth | 1:fe4a0b47ff25 | 30 | void servoBegin(void); |
tashworth | 0:1b64a0cedc5d | 31 | void initServoDriver(void); |
tashworth | 3:b7b4780a7f6e | 32 | void setServoPulse(uint8_t n, int angle); |
tashworth | 3:b7b4780a7f6e | 33 | void setServoPulseNo_delay(uint8_t n, int angle); |
tashworth | 0:1b64a0cedc5d | 34 | void servoPosition(int set); |
tashworth | 3:b7b4780a7f6e | 35 | void setGripper(int open); |
tashworth | 0:1b64a0cedc5d | 36 | |
tashworth | 0:1b64a0cedc5d | 37 | /************ |
tashworth | 0:1b64a0cedc5d | 38 | Variables for Servos |
tashworth | 0:1b64a0cedc5d | 39 | *************/ |
tashworth | 3:b7b4780a7f6e | 40 | int servoNum, servoAngle, outputDisabled, posNum, testVal; |
tashworth | 3:b7b4780a7f6e | 41 | int currentPosition[7]; |
tashworth | 0:1b64a0cedc5d | 42 | |
tashworth | 3:b7b4780a7f6e | 43 | typedef struct { |
tashworth | 3:b7b4780a7f6e | 44 | int arm_action; |
tashworth | 3:b7b4780a7f6e | 45 | int base_rotate; |
tashworth | 3:b7b4780a7f6e | 46 | int base_arm; |
tashworth | 3:b7b4780a7f6e | 47 | int lil_arm; |
tashworth | 3:b7b4780a7f6e | 48 | int big_arm; |
tashworth | 3:b7b4780a7f6e | 49 | int claw_arm; |
tashworth | 3:b7b4780a7f6e | 50 | int claw_rotate; |
tashworth | 3:b7b4780a7f6e | 51 | int claw_open; |
tashworth | 3:b7b4780a7f6e | 52 | } Coord; |
tashworth | 3:b7b4780a7f6e | 53 | |
tashworth | 3:b7b4780a7f6e | 54 | Coord Arm_Table[] = { |
tashworth | 3:b7b4780a7f6e | 55 | // POSITION ODER: |
tashworth | 3:b7b4780a7f6e | 56 | // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open |
tashworth | 3:b7b4780a7f6e | 57 | |
tashworth | 5:429e9a998bab | 58 | {STORE_POSITION, 90, 0, 177, 0, 0, 90, 90}, // storing position |
tashworth | 5:429e9a998bab | 59 | {OIL_RIG1, 90, 60, 130, 75, 60, 90, 90}, // point laser at oilrig1 |
tashworth | 5:429e9a998bab | 60 | {OIL_RIG2, 120, 60, 130, 75, 60, 90, 90}, // point laser at oilrig2 |
tashworth | 5:429e9a998bab | 61 | {OIL_RIG3, 135, 60, 130, 75, 60, 90, 90}, // point laser at oilrig2 |
tashworth | 3:b7b4780a7f6e | 62 | |
tashworth | 3:b7b4780a7f6e | 63 | }; |
tashworth | 3:b7b4780a7f6e | 64 | |
tashworth | 3:b7b4780a7f6e | 65 | |
tashworth | 3:b7b4780a7f6e | 66 | /* Variables for imageprocessing and distance */ |
tashworth | 3:b7b4780a7f6e | 67 | int x_coord; |
tashworth | 3:b7b4780a7f6e | 68 | int y_coord; |
tashworth | 3:b7b4780a7f6e | 69 | int area; |
tashworth | 3:b7b4780a7f6e | 70 | int shape; |
tashworth | 3:b7b4780a7f6e | 71 | |
tashworth | 0:1b64a0cedc5d | 72 | |
tashworth | 3:b7b4780a7f6e | 73 | |
tashworth | 3:b7b4780a7f6e | 74 | |
tashworth | 3:b7b4780a7f6e | 75 | |
tashworth | 0:1b64a0cedc5d | 76 | |
tashworth | 3:b7b4780a7f6e | 77 | int main() |
tashworth | 3:b7b4780a7f6e | 78 | { |
tashworth | 3:b7b4780a7f6e | 79 | |
tashworth | 3:b7b4780a7f6e | 80 | /***************** |
tashworth | 3:b7b4780a7f6e | 81 | INITIALIZATIONS |
tashworth | 3:b7b4780a7f6e | 82 | *******************/ |
tashworth | 3:b7b4780a7f6e | 83 | pc.baud(115200); |
tashworth | 2:4e082e4c255d | 84 | //Laser Range Finder Initialization |
tashworth | 3:b7b4780a7f6e | 85 | lrf_baudCalibration(); |
tashworth | 3:b7b4780a7f6e | 86 | |
tashworth | 2:4e082e4c255d | 87 | //Servo initialization |
tashworth | 3:b7b4780a7f6e | 88 | initServoDriver(); |
tashworth | 5:429e9a998bab | 89 | servoBegin(); // initiates servos to start position |
tashworth | 3:b7b4780a7f6e | 90 | //ServoOutputDisable = 0; |
tashworth | 5:429e9a998bab | 91 | while(1){ |
tashworth | 5:429e9a998bab | 92 | //pc.scanf("%d %d", &servoNum, &servoAngle); |
tashworth | 5:429e9a998bab | 93 | //setServoPulse(servoNum, servoAngle); |
tashworth | 5:429e9a998bab | 94 | pc.scanf("%d", &posNum); |
tashworth | 5:429e9a998bab | 95 | servoPosition(posNum); |
tashworth | 5:429e9a998bab | 96 | |
tashworth | 5:429e9a998bab | 97 | |
tashworth | 5:429e9a998bab | 98 | } |
tashworth | 3:b7b4780a7f6e | 99 | |
tashworth | 5:429e9a998bab | 100 | |
tashworth | 5:429e9a998bab | 101 | /* |
tashworth | 3:b7b4780a7f6e | 102 | while(1) { |
tashworth | 3:b7b4780a7f6e | 103 | pc.printf("PICK A TEST TO PERFORM\n"); |
tashworth | 3:b7b4780a7f6e | 104 | pc.printf("1) Distance Reading\n"); |
tashworth | 3:b7b4780a7f6e | 105 | pc.printf("2) Shape Detection\n"); |
tashworth | 3:b7b4780a7f6e | 106 | pc.printf("3) Servo Control\n"); |
tashworth | 3:b7b4780a7f6e | 107 | pc.printf("4) Motor Control\n"); |
tashworth | 3:b7b4780a7f6e | 108 | |
tashworth | 3:b7b4780a7f6e | 109 | pc.scanf("%d", &testVal); |
tashworth | 2:4e082e4c255d | 110 | |
tashworth | 3:b7b4780a7f6e | 111 | switch (testVal) { |
tashworth | 3:b7b4780a7f6e | 112 | case 1: |
tashworth | 3:b7b4780a7f6e | 113 | pc.printf("Distance = %d\n", getDistance()); |
tashworth | 3:b7b4780a7f6e | 114 | break; |
tashworth | 3:b7b4780a7f6e | 115 | case 2: |
tashworth | 3:b7b4780a7f6e | 116 | shape = shapeDetection_mass(); |
tashworth | 3:b7b4780a7f6e | 117 | printImageToFile(BINARY); |
tashworth | 3:b7b4780a7f6e | 118 | break; |
tashworth | 3:b7b4780a7f6e | 119 | case 3: |
tashworth | 3:b7b4780a7f6e | 120 | servoBegin(); |
tashworth | 3:b7b4780a7f6e | 121 | pc.scanf("%d %d", &servoNum, &servoAngle); |
tashworth | 3:b7b4780a7f6e | 122 | setServoPulse(servoNum, servoAngle); |
tashworth | 3:b7b4780a7f6e | 123 | break; |
tashworth | 3:b7b4780a7f6e | 124 | default: |
tashworth | 3:b7b4780a7f6e | 125 | pc.printf("ERROR: NOT A VALID TEST PROCEDURE"); |
tashworth | 3:b7b4780a7f6e | 126 | break; |
tashworth | 5:429e9a998bab | 127 | }*/ |
tashworth | 3:b7b4780a7f6e | 128 | |
tashworth | 3:b7b4780a7f6e | 129 | |
tashworth | 3:b7b4780a7f6e | 130 | /*if(pc.readable()) { |
tashworth | 3:b7b4780a7f6e | 131 | |
tashworth | 3:b7b4780a7f6e | 132 | pc.scanf("%d %d", &servoNum, &pulseWidth); |
tashworth | 3:b7b4780a7f6e | 133 | setServoPulse(servoNum, pulseWidth); |
tashworth | 3:b7b4780a7f6e | 134 | |
tashworth | 3:b7b4780a7f6e | 135 | |
tashworth | 3:b7b4780a7f6e | 136 | // pc.scanf("%d", &posNum); |
tashworth | 3:b7b4780a7f6e | 137 | //servoPosition(posNum); |
tashworth | 3:b7b4780a7f6e | 138 | |
tashworth | 0:1b64a0cedc5d | 139 | |
tashworth | 5:429e9a998bab | 140 | } |
tashworth | 5:429e9a998bab | 141 | }*/ |
tashworth | 3:b7b4780a7f6e | 142 | |
tashworth | 3:b7b4780a7f6e | 143 | /************************************************** |
tashworth | 3:b7b4780a7f6e | 144 | * FIRST STAGE |
tashworth | 3:b7b4780a7f6e | 145 | * |
tashworth | 3:b7b4780a7f6e | 146 | * - DETERMINE OIL RIG ON FIRE |
tashworth | 3:b7b4780a7f6e | 147 | * - DETERMINE PATH |
tashworth | 3:b7b4780a7f6e | 148 | * |
tashworth | 3:b7b4780a7f6e | 149 | **************************************************/ |
tashworth | 3:b7b4780a7f6e | 150 | |
tashworth | 0:1b64a0cedc5d | 151 | //TODO: EXTEND ARM AND FACE OILRIGS |
tashworth | 3:b7b4780a7f6e | 152 | |
tashworth | 0:1b64a0cedc5d | 153 | //OILRIG 1 DISTANCE READING |
tashworth | 0:1b64a0cedc5d | 154 | |
tashworth | 0:1b64a0cedc5d | 155 | //TODO: ROTATE ARM TO NEXT OIL RIG |
tashworth | 3:b7b4780a7f6e | 156 | |
tashworth | 3:b7b4780a7f6e | 157 | //OILRIG 2 DISTANCE READING |
tashworth | 3:b7b4780a7f6e | 158 | |
tashworth | 0:1b64a0cedc5d | 159 | //ROTATE ARM TO NEXT OIL RIG |
tashworth | 3:b7b4780a7f6e | 160 | |
tashworth | 0:1b64a0cedc5d | 161 | //OILRIG 3 DISTANCE READING |
tashworth | 0:1b64a0cedc5d | 162 | |
tashworth | 0:1b64a0cedc5d | 163 | } |
tashworth | 0:1b64a0cedc5d | 164 | |
tashworth | 0:1b64a0cedc5d | 165 | |
tashworth | 0:1b64a0cedc5d | 166 | |
tashworth | 0:1b64a0cedc5d | 167 | /************ |
tashworth | 0:1b64a0cedc5d | 168 | |
tashworth | 0:1b64a0cedc5d | 169 | Servo Functions |
tashworth | 0:1b64a0cedc5d | 170 | |
tashworth | 0:1b64a0cedc5d | 171 | **************/ |
tashworth | 0:1b64a0cedc5d | 172 | |
tashworth | 3:b7b4780a7f6e | 173 | void setServoPulse(uint8_t n, int angle) |
tashworth | 3:b7b4780a7f6e | 174 | { |
tashworth | 3:b7b4780a7f6e | 175 | int pulse; |
tashworth | 3:b7b4780a7f6e | 176 | pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); |
tashworth | 1:fe4a0b47ff25 | 177 | float pulselength = 20000; // 20,000 us per second |
tashworth | 1:fe4a0b47ff25 | 178 | int i = currentPosition[n]; |
tashworth | 3:b7b4780a7f6e | 179 | pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]); |
tashworth | 3:b7b4780a7f6e | 180 | int pulse2; |
tashworth | 3:b7b4780a7f6e | 181 | if(currentPosition[n] < pulse) { |
tashworth | 3:b7b4780a7f6e | 182 | for(i; i < pulse; i++) { |
tashworth | 3:b7b4780a7f6e | 183 | pulse2 = 4094 * i / pulselength; |
tashworth | 3:b7b4780a7f6e | 184 | pwm.setPWM(n, 0, pulse2); |
tashworth | 3:b7b4780a7f6e | 185 | wait_ms(3); |
tashworth | 3:b7b4780a7f6e | 186 | } |
tashworth | 1:fe4a0b47ff25 | 187 | } else if (currentPosition[n] > pulse) { |
tashworth | 3:b7b4780a7f6e | 188 | for(i; i > pulse; i--) { |
tashworth | 3:b7b4780a7f6e | 189 | pulse2 = 4094 * i / pulselength; |
tashworth | 3:b7b4780a7f6e | 190 | pwm.setPWM(n, 0, pulse2); |
tashworth | 3:b7b4780a7f6e | 191 | wait_ms(3); |
tashworth | 3:b7b4780a7f6e | 192 | } |
tashworth | 1:fe4a0b47ff25 | 193 | } |
tashworth | 1:fe4a0b47ff25 | 194 | currentPosition[n] = i; |
tashworth | 3:b7b4780a7f6e | 195 | pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle); |
tashworth | 0:1b64a0cedc5d | 196 | } |
tashworth | 0:1b64a0cedc5d | 197 | |
tashworth | 3:b7b4780a7f6e | 198 | void initServoDriver(void) |
tashworth | 3:b7b4780a7f6e | 199 | { |
tashworth | 0:1b64a0cedc5d | 200 | pwm.begin(); |
tashworth | 0:1b64a0cedc5d | 201 | //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale(). |
tashworth | 0:1b64a0cedc5d | 202 | pwm.setPrescale(140); //This value is decided for 20ms interval. |
tashworth | 0:1b64a0cedc5d | 203 | pwm.setI2Cfreq(400000); //400kHz |
tashworth | 3:b7b4780a7f6e | 204 | |
tashworth | 3:b7b4780a7f6e | 205 | } |
tashworth | 3:b7b4780a7f6e | 206 | |
tashworth | 3:b7b4780a7f6e | 207 | void servoBegin(void) |
tashworth | 3:b7b4780a7f6e | 208 | { |
tashworth | 3:b7b4780a7f6e | 209 | pc.printf("Setting Initial Position\n\r"); |
tashworth | 3:b7b4780a7f6e | 210 | setServoPulseNo_delay(0, 90); |
tashworth | 5:429e9a998bab | 211 | setServoPulseNo_delay(1, 0); |
tashworth | 5:429e9a998bab | 212 | setServoPulseNo_delay(2, 177); |
tashworth | 5:429e9a998bab | 213 | setServoPulseNo_delay(3, 0); |
tashworth | 5:429e9a998bab | 214 | setServoPulseNo_delay(4, 0); |
tashworth | 3:b7b4780a7f6e | 215 | setServoPulseNo_delay(5, 90); |
tashworth | 3:b7b4780a7f6e | 216 | setGripper(1); |
tashworth | 0:1b64a0cedc5d | 217 | } |
tashworth | 0:1b64a0cedc5d | 218 | |
tashworth | 3:b7b4780a7f6e | 219 | void setServoPulseNo_delay(uint8_t n, int angle) |
tashworth | 3:b7b4780a7f6e | 220 | { |
tashworth | 3:b7b4780a7f6e | 221 | int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); |
tashworth | 1:fe4a0b47ff25 | 222 | float pulselength = 20000; // 20,000 us per second |
tashworth | 1:fe4a0b47ff25 | 223 | currentPosition[n] = pulse; |
tashworth | 3:b7b4780a7f6e | 224 | pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle); |
tashworth | 1:fe4a0b47ff25 | 225 | pulse = 4094 * pulse / pulselength; |
tashworth | 1:fe4a0b47ff25 | 226 | pwm.setPWM(n, 0, pulse); |
tashworth | 3:b7b4780a7f6e | 227 | |
tashworth | 1:fe4a0b47ff25 | 228 | } |
tashworth | 3:b7b4780a7f6e | 229 | |
tashworth | 3:b7b4780a7f6e | 230 | void setGripper(int open) |
tashworth | 3:b7b4780a7f6e | 231 | { |
tashworth | 3:b7b4780a7f6e | 232 | if (open) { |
tashworth | 3:b7b4780a7f6e | 233 | pc.printf("Gripper Open\r"); |
tashworth | 3:b7b4780a7f6e | 234 | setServoPulseNo_delay(6, 10); |
tashworth | 3:b7b4780a7f6e | 235 | } else { |
tashworth | 3:b7b4780a7f6e | 236 | pc.printf("Gripper Closed\n\r"); |
tashworth | 3:b7b4780a7f6e | 237 | setServoPulseNo_delay(6, 170); |
tashworth | 3:b7b4780a7f6e | 238 | } |
tashworth | 3:b7b4780a7f6e | 239 | } |
tashworth | 3:b7b4780a7f6e | 240 | |
tashworth | 3:b7b4780a7f6e | 241 | void servoPosition(int set) |
tashworth | 3:b7b4780a7f6e | 242 | { |
tashworth | 3:b7b4780a7f6e | 243 | //moves to current position |
tashworth | 3:b7b4780a7f6e | 244 | setServoPulse(0, Arm_Table[set].base_rotate); |
tashworth | 3:b7b4780a7f6e | 245 | setServoPulse(1, Arm_Table[set].base_arm); |
tashworth | 3:b7b4780a7f6e | 246 | setServoPulse(2, Arm_Table[set].lil_arm); |
tashworth | 3:b7b4780a7f6e | 247 | setServoPulse(3, Arm_Table[set].big_arm); |
tashworth | 3:b7b4780a7f6e | 248 | setServoPulse(4, Arm_Table[set].claw_arm); |
tashworth | 3:b7b4780a7f6e | 249 | setServoPulse(5, Arm_Table[set].claw_rotate); |
tashworth | 3:b7b4780a7f6e | 250 | setServoPulse(6, Arm_Table[set].claw_open); |
tashworth | 0:1b64a0cedc5d | 251 | } |
tashworth | 0:1b64a0cedc5d | 252 | |
tashworth | 0:1b64a0cedc5d | 253 | |
tashworth | 0:1b64a0cedc5d | 254 |