Thomas Ashworth / theRobot

For IEEE Robotics

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

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?

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 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