Paolina Povolotskaya / theRobotNEW

ksdflsjdfljas

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

Fork of theRobot by Thomas Ashworth

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?

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