Thomas Ashworth / theRobot

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?

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