nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
main.cpp@4:42460f387701, 2014-03-13 (annotated)
- Committer:
- tashworth
- Date:
- Thu Mar 13 17:10:42 2014 +0000
- Revision:
- 4:42460f387701
- Parent:
- 3:b7b4780a7f6e
- Child:
- 5:429e9a998bab
working:; laser distance measurement; shape detection; center of mass coordinates; area of shape calculation; servos normalized to degrees 0-180
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 | 3:b7b4780a7f6e | 58 | {STORE_POSITION, 900, 900, 900, 900, 900, 900, 900}, // storing position |
tashworth | 2:4e082e4c255d | 59 | {OIL_RIG1, 1500, 1400, 1900, 900, 900, 0, 0}, // point laser at oilrig2 |
tashworth | 3:b7b4780a7f6e | 60 | |
tashworth | 3:b7b4780a7f6e | 61 | }; |
tashworth | 3:b7b4780a7f6e | 62 | |
tashworth | 3:b7b4780a7f6e | 63 | |
tashworth | 3:b7b4780a7f6e | 64 | /* Variables for imageprocessing and distance */ |
tashworth | 3:b7b4780a7f6e | 65 | int x_coord; |
tashworth | 3:b7b4780a7f6e | 66 | int y_coord; |
tashworth | 3:b7b4780a7f6e | 67 | int area; |
tashworth | 3:b7b4780a7f6e | 68 | int shape; |
tashworth | 3:b7b4780a7f6e | 69 | |
tashworth | 0:1b64a0cedc5d | 70 | |
tashworth | 3:b7b4780a7f6e | 71 | |
tashworth | 3:b7b4780a7f6e | 72 | |
tashworth | 3:b7b4780a7f6e | 73 | |
tashworth | 0:1b64a0cedc5d | 74 | |
tashworth | 3:b7b4780a7f6e | 75 | int main() |
tashworth | 3:b7b4780a7f6e | 76 | { |
tashworth | 3:b7b4780a7f6e | 77 | |
tashworth | 3:b7b4780a7f6e | 78 | /***************** |
tashworth | 3:b7b4780a7f6e | 79 | INITIALIZATIONS |
tashworth | 3:b7b4780a7f6e | 80 | *******************/ |
tashworth | 3:b7b4780a7f6e | 81 | pc.baud(115200); |
tashworth | 2:4e082e4c255d | 82 | //Laser Range Finder Initialization |
tashworth | 3:b7b4780a7f6e | 83 | lrf_baudCalibration(); |
tashworth | 3:b7b4780a7f6e | 84 | |
tashworth | 2:4e082e4c255d | 85 | //Servo initialization |
tashworth | 3:b7b4780a7f6e | 86 | initServoDriver(); |
tashworth | 3:b7b4780a7f6e | 87 | // servoBegin(); // initiates servos to start position |
tashworth | 3:b7b4780a7f6e | 88 | //ServoOutputDisable = 0; |
tashworth | 3:b7b4780a7f6e | 89 | |
tashworth | 3:b7b4780a7f6e | 90 | while(1) { |
tashworth | 3:b7b4780a7f6e | 91 | pc.printf("PICK A TEST TO PERFORM\n"); |
tashworth | 3:b7b4780a7f6e | 92 | pc.printf("1) Distance Reading\n"); |
tashworth | 3:b7b4780a7f6e | 93 | pc.printf("2) Shape Detection\n"); |
tashworth | 3:b7b4780a7f6e | 94 | pc.printf("3) Servo Control\n"); |
tashworth | 3:b7b4780a7f6e | 95 | pc.printf("4) Motor Control\n"); |
tashworth | 3:b7b4780a7f6e | 96 | |
tashworth | 3:b7b4780a7f6e | 97 | pc.scanf("%d", &testVal); |
tashworth | 2:4e082e4c255d | 98 | |
tashworth | 3:b7b4780a7f6e | 99 | switch (testVal) { |
tashworth | 3:b7b4780a7f6e | 100 | case 1: |
tashworth | 3:b7b4780a7f6e | 101 | pc.printf("Distance = %d\n", getDistance()); |
tashworth | 3:b7b4780a7f6e | 102 | break; |
tashworth | 3:b7b4780a7f6e | 103 | case 2: |
tashworth | 3:b7b4780a7f6e | 104 | shape = shapeDetection_mass(); |
tashworth | 3:b7b4780a7f6e | 105 | printImageToFile(BINARY); |
tashworth | 3:b7b4780a7f6e | 106 | break; |
tashworth | 3:b7b4780a7f6e | 107 | case 3: |
tashworth | 3:b7b4780a7f6e | 108 | servoBegin(); |
tashworth | 3:b7b4780a7f6e | 109 | pc.scanf("%d %d", &servoNum, &servoAngle); |
tashworth | 3:b7b4780a7f6e | 110 | setServoPulse(servoNum, servoAngle); |
tashworth | 3:b7b4780a7f6e | 111 | break; |
tashworth | 3:b7b4780a7f6e | 112 | default: |
tashworth | 3:b7b4780a7f6e | 113 | pc.printf("ERROR: NOT A VALID TEST PROCEDURE"); |
tashworth | 3:b7b4780a7f6e | 114 | break; |
tashworth | 0:1b64a0cedc5d | 115 | } |
tashworth | 3:b7b4780a7f6e | 116 | |
tashworth | 3:b7b4780a7f6e | 117 | |
tashworth | 3:b7b4780a7f6e | 118 | /*if(pc.readable()) { |
tashworth | 3:b7b4780a7f6e | 119 | |
tashworth | 3:b7b4780a7f6e | 120 | pc.scanf("%d %d", &servoNum, &pulseWidth); |
tashworth | 3:b7b4780a7f6e | 121 | setServoPulse(servoNum, pulseWidth); |
tashworth | 3:b7b4780a7f6e | 122 | |
tashworth | 3:b7b4780a7f6e | 123 | |
tashworth | 3:b7b4780a7f6e | 124 | // pc.scanf("%d", &posNum); |
tashworth | 3:b7b4780a7f6e | 125 | //servoPosition(posNum); |
tashworth | 3:b7b4780a7f6e | 126 | |
tashworth | 0:1b64a0cedc5d | 127 | |
tashworth | 3:b7b4780a7f6e | 128 | }*/ |
tashworth | 3:b7b4780a7f6e | 129 | } |
tashworth | 3:b7b4780a7f6e | 130 | |
tashworth | 3:b7b4780a7f6e | 131 | /************************************************** |
tashworth | 3:b7b4780a7f6e | 132 | * FIRST STAGE |
tashworth | 3:b7b4780a7f6e | 133 | * |
tashworth | 3:b7b4780a7f6e | 134 | * - DETERMINE OIL RIG ON FIRE |
tashworth | 3:b7b4780a7f6e | 135 | * - DETERMINE PATH |
tashworth | 3:b7b4780a7f6e | 136 | * |
tashworth | 3:b7b4780a7f6e | 137 | **************************************************/ |
tashworth | 3:b7b4780a7f6e | 138 | |
tashworth | 0:1b64a0cedc5d | 139 | //TODO: EXTEND ARM AND FACE OILRIGS |
tashworth | 3:b7b4780a7f6e | 140 | |
tashworth | 0:1b64a0cedc5d | 141 | //OILRIG 1 DISTANCE READING |
tashworth | 0:1b64a0cedc5d | 142 | |
tashworth | 0:1b64a0cedc5d | 143 | //TODO: ROTATE ARM TO NEXT OIL RIG |
tashworth | 3:b7b4780a7f6e | 144 | |
tashworth | 3:b7b4780a7f6e | 145 | //OILRIG 2 DISTANCE READING |
tashworth | 3:b7b4780a7f6e | 146 | |
tashworth | 0:1b64a0cedc5d | 147 | //ROTATE ARM TO NEXT OIL RIG |
tashworth | 3:b7b4780a7f6e | 148 | |
tashworth | 0:1b64a0cedc5d | 149 | //OILRIG 3 DISTANCE READING |
tashworth | 0:1b64a0cedc5d | 150 | |
tashworth | 0:1b64a0cedc5d | 151 | } |
tashworth | 0:1b64a0cedc5d | 152 | |
tashworth | 0:1b64a0cedc5d | 153 | |
tashworth | 0:1b64a0cedc5d | 154 | |
tashworth | 0:1b64a0cedc5d | 155 | /************ |
tashworth | 0:1b64a0cedc5d | 156 | |
tashworth | 0:1b64a0cedc5d | 157 | Servo Functions |
tashworth | 0:1b64a0cedc5d | 158 | |
tashworth | 0:1b64a0cedc5d | 159 | **************/ |
tashworth | 0:1b64a0cedc5d | 160 | |
tashworth | 3:b7b4780a7f6e | 161 | void setServoPulse(uint8_t n, int angle) |
tashworth | 3:b7b4780a7f6e | 162 | { |
tashworth | 3:b7b4780a7f6e | 163 | int pulse; |
tashworth | 3:b7b4780a7f6e | 164 | pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); |
tashworth | 1:fe4a0b47ff25 | 165 | float pulselength = 20000; // 20,000 us per second |
tashworth | 1:fe4a0b47ff25 | 166 | int i = currentPosition[n]; |
tashworth | 3:b7b4780a7f6e | 167 | pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]); |
tashworth | 3:b7b4780a7f6e | 168 | int pulse2; |
tashworth | 3:b7b4780a7f6e | 169 | if(currentPosition[n] < pulse) { |
tashworth | 3:b7b4780a7f6e | 170 | for(i; i < pulse; i++) { |
tashworth | 3:b7b4780a7f6e | 171 | pulse2 = 4094 * i / pulselength; |
tashworth | 3:b7b4780a7f6e | 172 | pwm.setPWM(n, 0, pulse2); |
tashworth | 3:b7b4780a7f6e | 173 | wait_ms(3); |
tashworth | 3:b7b4780a7f6e | 174 | } |
tashworth | 1:fe4a0b47ff25 | 175 | } else if (currentPosition[n] > pulse) { |
tashworth | 3:b7b4780a7f6e | 176 | for(i; i > pulse; i--) { |
tashworth | 3:b7b4780a7f6e | 177 | pulse2 = 4094 * i / pulselength; |
tashworth | 3:b7b4780a7f6e | 178 | pwm.setPWM(n, 0, pulse2); |
tashworth | 3:b7b4780a7f6e | 179 | wait_ms(3); |
tashworth | 3:b7b4780a7f6e | 180 | } |
tashworth | 1:fe4a0b47ff25 | 181 | } |
tashworth | 1:fe4a0b47ff25 | 182 | currentPosition[n] = i; |
tashworth | 3:b7b4780a7f6e | 183 | pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle); |
tashworth | 0:1b64a0cedc5d | 184 | } |
tashworth | 0:1b64a0cedc5d | 185 | |
tashworth | 3:b7b4780a7f6e | 186 | void initServoDriver(void) |
tashworth | 3:b7b4780a7f6e | 187 | { |
tashworth | 0:1b64a0cedc5d | 188 | pwm.begin(); |
tashworth | 0:1b64a0cedc5d | 189 | //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale(). |
tashworth | 0:1b64a0cedc5d | 190 | pwm.setPrescale(140); //This value is decided for 20ms interval. |
tashworth | 0:1b64a0cedc5d | 191 | pwm.setI2Cfreq(400000); //400kHz |
tashworth | 3:b7b4780a7f6e | 192 | |
tashworth | 3:b7b4780a7f6e | 193 | } |
tashworth | 3:b7b4780a7f6e | 194 | |
tashworth | 3:b7b4780a7f6e | 195 | void servoBegin(void) |
tashworth | 3:b7b4780a7f6e | 196 | { |
tashworth | 3:b7b4780a7f6e | 197 | pc.printf("Setting Initial Position\n\r"); |
tashworth | 3:b7b4780a7f6e | 198 | setServoPulseNo_delay(0, 90); |
tashworth | 3:b7b4780a7f6e | 199 | setServoPulseNo_delay(1, 90); |
tashworth | 3:b7b4780a7f6e | 200 | setServoPulseNo_delay(2, 90); |
tashworth | 3:b7b4780a7f6e | 201 | setServoPulseNo_delay(3, 90); |
tashworth | 3:b7b4780a7f6e | 202 | setServoPulseNo_delay(4, 90); |
tashworth | 3:b7b4780a7f6e | 203 | setServoPulseNo_delay(5, 90); |
tashworth | 3:b7b4780a7f6e | 204 | setGripper(1); |
tashworth | 0:1b64a0cedc5d | 205 | } |
tashworth | 0:1b64a0cedc5d | 206 | |
tashworth | 3:b7b4780a7f6e | 207 | void setServoPulseNo_delay(uint8_t n, int angle) |
tashworth | 3:b7b4780a7f6e | 208 | { |
tashworth | 3:b7b4780a7f6e | 209 | int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); |
tashworth | 1:fe4a0b47ff25 | 210 | float pulselength = 20000; // 20,000 us per second |
tashworth | 1:fe4a0b47ff25 | 211 | currentPosition[n] = pulse; |
tashworth | 3:b7b4780a7f6e | 212 | pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle); |
tashworth | 1:fe4a0b47ff25 | 213 | pulse = 4094 * pulse / pulselength; |
tashworth | 1:fe4a0b47ff25 | 214 | pwm.setPWM(n, 0, pulse); |
tashworth | 3:b7b4780a7f6e | 215 | |
tashworth | 1:fe4a0b47ff25 | 216 | } |
tashworth | 3:b7b4780a7f6e | 217 | |
tashworth | 3:b7b4780a7f6e | 218 | void setGripper(int open) |
tashworth | 3:b7b4780a7f6e | 219 | { |
tashworth | 3:b7b4780a7f6e | 220 | if (open) { |
tashworth | 3:b7b4780a7f6e | 221 | pc.printf("Gripper Open\r"); |
tashworth | 3:b7b4780a7f6e | 222 | setServoPulseNo_delay(6, 10); |
tashworth | 3:b7b4780a7f6e | 223 | } else { |
tashworth | 3:b7b4780a7f6e | 224 | pc.printf("Gripper Closed\n\r"); |
tashworth | 3:b7b4780a7f6e | 225 | setServoPulseNo_delay(6, 170); |
tashworth | 3:b7b4780a7f6e | 226 | } |
tashworth | 3:b7b4780a7f6e | 227 | } |
tashworth | 3:b7b4780a7f6e | 228 | |
tashworth | 3:b7b4780a7f6e | 229 | void servoPosition(int set) |
tashworth | 3:b7b4780a7f6e | 230 | { |
tashworth | 3:b7b4780a7f6e | 231 | //moves to current position |
tashworth | 3:b7b4780a7f6e | 232 | setServoPulse(0, Arm_Table[set].base_rotate); |
tashworth | 3:b7b4780a7f6e | 233 | setServoPulse(1, Arm_Table[set].base_arm); |
tashworth | 3:b7b4780a7f6e | 234 | setServoPulse(2, Arm_Table[set].lil_arm); |
tashworth | 3:b7b4780a7f6e | 235 | setServoPulse(3, Arm_Table[set].big_arm); |
tashworth | 3:b7b4780a7f6e | 236 | setServoPulse(4, Arm_Table[set].claw_arm); |
tashworth | 3:b7b4780a7f6e | 237 | setServoPulse(5, Arm_Table[set].claw_rotate); |
tashworth | 3:b7b4780a7f6e | 238 | setServoPulse(6, Arm_Table[set].claw_open); |
tashworth | 0:1b64a0cedc5d | 239 | } |
tashworth | 0:1b64a0cedc5d | 240 | |
tashworth | 0:1b64a0cedc5d | 241 | |
tashworth | 0:1b64a0cedc5d | 242 |