For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
main.cpp@3:b7b4780a7f6e, 2014-03-13 (annotated)
- Committer:
- tashworth
- Date:
- Thu Mar 13 17:06:34 2014 +0000
- Revision:
- 3:b7b4780a7f6e
- Parent:
- 2:4e082e4c255d
- Child:
- 4:42460f387701
3-13-14; - working servo normalized to degrees 0-180; - working shape detection with camera 11" from ground plane; - working area of shape calculation; - working center of mass; - writes binary, decimal, or hex .dat and.txt file of the image to local
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 | pc.printf("Distance = %d", getDistance()); |
tashworth | 3:b7b4780a7f6e | 119 | |
tashworth | 3:b7b4780a7f6e | 120 | /*if(pc.readable()) { |
tashworth | 3:b7b4780a7f6e | 121 | |
tashworth | 3:b7b4780a7f6e | 122 | pc.scanf("%d %d", &servoNum, &pulseWidth); |
tashworth | 3:b7b4780a7f6e | 123 | setServoPulse(servoNum, pulseWidth); |
tashworth | 3:b7b4780a7f6e | 124 | |
tashworth | 3:b7b4780a7f6e | 125 | |
tashworth | 3:b7b4780a7f6e | 126 | // pc.scanf("%d", &posNum); |
tashworth | 3:b7b4780a7f6e | 127 | //servoPosition(posNum); |
tashworth | 3:b7b4780a7f6e | 128 | |
tashworth | 0:1b64a0cedc5d | 129 | |
tashworth | 3:b7b4780a7f6e | 130 | }*/ |
tashworth | 3:b7b4780a7f6e | 131 | } |
tashworth | 3:b7b4780a7f6e | 132 | |
tashworth | 3:b7b4780a7f6e | 133 | /************************************************** |
tashworth | 3:b7b4780a7f6e | 134 | * FIRST STAGE |
tashworth | 3:b7b4780a7f6e | 135 | * |
tashworth | 3:b7b4780a7f6e | 136 | * - DETERMINE OIL RIG ON FIRE |
tashworth | 3:b7b4780a7f6e | 137 | * - DETERMINE PATH |
tashworth | 3:b7b4780a7f6e | 138 | * |
tashworth | 3:b7b4780a7f6e | 139 | **************************************************/ |
tashworth | 3:b7b4780a7f6e | 140 | |
tashworth | 0:1b64a0cedc5d | 141 | //TODO: EXTEND ARM AND FACE OILRIGS |
tashworth | 3:b7b4780a7f6e | 142 | |
tashworth | 0:1b64a0cedc5d | 143 | //OILRIG 1 DISTANCE READING |
tashworth | 0:1b64a0cedc5d | 144 | |
tashworth | 0:1b64a0cedc5d | 145 | //TODO: ROTATE ARM TO NEXT OIL RIG |
tashworth | 3:b7b4780a7f6e | 146 | |
tashworth | 3:b7b4780a7f6e | 147 | //OILRIG 2 DISTANCE READING |
tashworth | 3:b7b4780a7f6e | 148 | |
tashworth | 0:1b64a0cedc5d | 149 | //ROTATE ARM TO NEXT OIL RIG |
tashworth | 3:b7b4780a7f6e | 150 | |
tashworth | 0:1b64a0cedc5d | 151 | //OILRIG 3 DISTANCE READING |
tashworth | 0:1b64a0cedc5d | 152 | |
tashworth | 0:1b64a0cedc5d | 153 | } |
tashworth | 0:1b64a0cedc5d | 154 | |
tashworth | 0:1b64a0cedc5d | 155 | |
tashworth | 0:1b64a0cedc5d | 156 | |
tashworth | 0:1b64a0cedc5d | 157 | /************ |
tashworth | 0:1b64a0cedc5d | 158 | |
tashworth | 0:1b64a0cedc5d | 159 | Servo Functions |
tashworth | 0:1b64a0cedc5d | 160 | |
tashworth | 0:1b64a0cedc5d | 161 | **************/ |
tashworth | 0:1b64a0cedc5d | 162 | |
tashworth | 3:b7b4780a7f6e | 163 | void setServoPulse(uint8_t n, int angle) |
tashworth | 3:b7b4780a7f6e | 164 | { |
tashworth | 3:b7b4780a7f6e | 165 | int pulse; |
tashworth | 3:b7b4780a7f6e | 166 | pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); |
tashworth | 1:fe4a0b47ff25 | 167 | float pulselength = 20000; // 20,000 us per second |
tashworth | 1:fe4a0b47ff25 | 168 | int i = currentPosition[n]; |
tashworth | 3:b7b4780a7f6e | 169 | pc.printf("ServoNumber: %d Begining Pulse: %d\n\r",n,currentPosition[n]); |
tashworth | 3:b7b4780a7f6e | 170 | int pulse2; |
tashworth | 3:b7b4780a7f6e | 171 | if(currentPosition[n] < pulse) { |
tashworth | 3:b7b4780a7f6e | 172 | for(i; i < pulse; i++) { |
tashworth | 3:b7b4780a7f6e | 173 | pulse2 = 4094 * i / pulselength; |
tashworth | 3:b7b4780a7f6e | 174 | pwm.setPWM(n, 0, pulse2); |
tashworth | 3:b7b4780a7f6e | 175 | wait_ms(3); |
tashworth | 3:b7b4780a7f6e | 176 | } |
tashworth | 1:fe4a0b47ff25 | 177 | } else if (currentPosition[n] > pulse) { |
tashworth | 3:b7b4780a7f6e | 178 | for(i; i > pulse; i--) { |
tashworth | 3:b7b4780a7f6e | 179 | pulse2 = 4094 * i / pulselength; |
tashworth | 3:b7b4780a7f6e | 180 | pwm.setPWM(n, 0, pulse2); |
tashworth | 3:b7b4780a7f6e | 181 | wait_ms(3); |
tashworth | 3:b7b4780a7f6e | 182 | } |
tashworth | 1:fe4a0b47ff25 | 183 | } |
tashworth | 1:fe4a0b47ff25 | 184 | currentPosition[n] = i; |
tashworth | 3:b7b4780a7f6e | 185 | pc.printf("\nEND: pulse: %d, angle: %d\n\n", i, angle); |
tashworth | 0:1b64a0cedc5d | 186 | } |
tashworth | 0:1b64a0cedc5d | 187 | |
tashworth | 3:b7b4780a7f6e | 188 | void initServoDriver(void) |
tashworth | 3:b7b4780a7f6e | 189 | { |
tashworth | 0:1b64a0cedc5d | 190 | pwm.begin(); |
tashworth | 0:1b64a0cedc5d | 191 | //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale(). |
tashworth | 0:1b64a0cedc5d | 192 | pwm.setPrescale(140); //This value is decided for 20ms interval. |
tashworth | 0:1b64a0cedc5d | 193 | pwm.setI2Cfreq(400000); //400kHz |
tashworth | 3:b7b4780a7f6e | 194 | |
tashworth | 3:b7b4780a7f6e | 195 | } |
tashworth | 3:b7b4780a7f6e | 196 | |
tashworth | 3:b7b4780a7f6e | 197 | void servoBegin(void) |
tashworth | 3:b7b4780a7f6e | 198 | { |
tashworth | 3:b7b4780a7f6e | 199 | pc.printf("Setting Initial Position\n\r"); |
tashworth | 3:b7b4780a7f6e | 200 | setServoPulseNo_delay(0, 90); |
tashworth | 3:b7b4780a7f6e | 201 | setServoPulseNo_delay(1, 90); |
tashworth | 3:b7b4780a7f6e | 202 | setServoPulseNo_delay(2, 90); |
tashworth | 3:b7b4780a7f6e | 203 | setServoPulseNo_delay(3, 90); |
tashworth | 3:b7b4780a7f6e | 204 | setServoPulseNo_delay(4, 90); |
tashworth | 3:b7b4780a7f6e | 205 | setServoPulseNo_delay(5, 90); |
tashworth | 3:b7b4780a7f6e | 206 | setGripper(1); |
tashworth | 0:1b64a0cedc5d | 207 | } |
tashworth | 0:1b64a0cedc5d | 208 | |
tashworth | 3:b7b4780a7f6e | 209 | void setServoPulseNo_delay(uint8_t n, int angle) |
tashworth | 3:b7b4780a7f6e | 210 | { |
tashworth | 3:b7b4780a7f6e | 211 | int pulse = MIN_SERVO_PULSE + (( angle * (MAX_SERVO_PULSE - MIN_SERVO_PULSE)) / SERVO_MAX_ANGLE); |
tashworth | 1:fe4a0b47ff25 | 212 | float pulselength = 20000; // 20,000 us per second |
tashworth | 1:fe4a0b47ff25 | 213 | currentPosition[n] = pulse; |
tashworth | 3:b7b4780a7f6e | 214 | pc.printf("ServoNumber: %d Pulsewidth: %d Angle: %d \n\r",n,pulse, angle); |
tashworth | 1:fe4a0b47ff25 | 215 | pulse = 4094 * pulse / pulselength; |
tashworth | 1:fe4a0b47ff25 | 216 | pwm.setPWM(n, 0, pulse); |
tashworth | 3:b7b4780a7f6e | 217 | |
tashworth | 1:fe4a0b47ff25 | 218 | } |
tashworth | 3:b7b4780a7f6e | 219 | |
tashworth | 3:b7b4780a7f6e | 220 | void setGripper(int open) |
tashworth | 3:b7b4780a7f6e | 221 | { |
tashworth | 3:b7b4780a7f6e | 222 | if (open) { |
tashworth | 3:b7b4780a7f6e | 223 | pc.printf("Gripper Open\r"); |
tashworth | 3:b7b4780a7f6e | 224 | setServoPulseNo_delay(6, 10); |
tashworth | 3:b7b4780a7f6e | 225 | } else { |
tashworth | 3:b7b4780a7f6e | 226 | pc.printf("Gripper Closed\n\r"); |
tashworth | 3:b7b4780a7f6e | 227 | setServoPulseNo_delay(6, 170); |
tashworth | 3:b7b4780a7f6e | 228 | } |
tashworth | 3:b7b4780a7f6e | 229 | } |
tashworth | 3:b7b4780a7f6e | 230 | |
tashworth | 3:b7b4780a7f6e | 231 | void servoPosition(int set) |
tashworth | 3:b7b4780a7f6e | 232 | { |
tashworth | 3:b7b4780a7f6e | 233 | //moves to current position |
tashworth | 3:b7b4780a7f6e | 234 | setServoPulse(0, Arm_Table[set].base_rotate); |
tashworth | 3:b7b4780a7f6e | 235 | setServoPulse(1, Arm_Table[set].base_arm); |
tashworth | 3:b7b4780a7f6e | 236 | setServoPulse(2, Arm_Table[set].lil_arm); |
tashworth | 3:b7b4780a7f6e | 237 | setServoPulse(3, Arm_Table[set].big_arm); |
tashworth | 3:b7b4780a7f6e | 238 | setServoPulse(4, Arm_Table[set].claw_arm); |
tashworth | 3:b7b4780a7f6e | 239 | setServoPulse(5, Arm_Table[set].claw_rotate); |
tashworth | 3:b7b4780a7f6e | 240 | setServoPulse(6, Arm_Table[set].claw_open); |
tashworth | 0:1b64a0cedc5d | 241 | } |
tashworth | 0:1b64a0cedc5d | 242 | |
tashworth | 0:1b64a0cedc5d | 243 | |
tashworth | 0:1b64a0cedc5d | 244 |