added normalization and use of keyboard. I tested it and it worked

Dependencies:   Adafruit-16-Ch-PWM-Servo-Driver mbed

Fork of theRobot by Thomas Ashworth

Committer:
Fairy_Paolina
Date:
Thu Mar 06 00:12:56 2014 +0000
Revision:
3:587441455259
Parent:
2:4e082e4c255d
Child:
4:116829a5ae3c
update

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 0:1b64a0cedc5d 6 Adafruit_PWMServoDriver pwm(p9,p10);
tashworth 0:1b64a0cedc5d 7 DigitalOut ServoOutputDisable(p8);
tashworth 0:1b64a0cedc5d 8 extern Serial lrf;
tashworth 0:1b64a0cedc5d 9
tashworth 0:1b64a0cedc5d 10
tashworth 0:1b64a0cedc5d 11
tashworth 0:1b64a0cedc5d 12 //Servo Positions
tashworth 0:1b64a0cedc5d 13 #define STORE_POSITION 0
tashworth 0:1b64a0cedc5d 14 #define OIL_RIG1 1
tashworth 0:1b64a0cedc5d 15 #define OIL_RIG2 2
tashworth 2:4e082e4c255d 16 #define OIL_RIG3 3
tashworth 2:4e082e4c255d 17 #define IMG_SHAPE_1 4
tashworth 2:4e082e4c255d 18 #define IMG_SHAPE_2 5
tashworth 2:4e082e4c255d 19 #define IMG_SHAPE_3 6
tashworth 2:4e082e4c255d 20 #define GRASP_SHAPE_1 7
tashworth 2:4e082e4c255d 21 #define GRASP_SHAPE_2 8
tashworth 2:4e082e4c255d 22 #define GRASP_SHAPE_3 9
tashworth 2:4e082e4c255d 23 #define INSERT_TOOL_1 10
tashworth 2:4e082e4c255d 24 #define INSERT_TOOL_2 11
tashworth 2:4e082e4c255d 25 #define INSERT_TOOL_3 11
tashworth 2:4e082e4c255d 26
tashworth 0:1b64a0cedc5d 27
tashworth 1:fe4a0b47ff25 28 void servoBegin(void);
tashworth 0:1b64a0cedc5d 29 void initServoDriver(void);
tashworth 0:1b64a0cedc5d 30 void setServoPulse(uint8_t n, float pulse);
tashworth 1:fe4a0b47ff25 31 void setServoPulseNo_delay(uint8_t n, float pulse);
tashworth 0:1b64a0cedc5d 32 void servoPosition(int set);
Fairy_Paolina 3:587441455259 33 int ServoTest(void);
tashworth 0:1b64a0cedc5d 34
tashworth 0:1b64a0cedc5d 35 /************
tashworth 0:1b64a0cedc5d 36 Variables for Servos
tashworth 0:1b64a0cedc5d 37 *************/
tashworth 0:1b64a0cedc5d 38 int servoNum, pulseWidth, outputDisabled, posNum;
tashworth 0:1b64a0cedc5d 39 int currentPosition[7];
tashworth 0:1b64a0cedc5d 40
tashworth 0:1b64a0cedc5d 41 typedef struct {int arm_action; int base_rotate; int base_arm;
tashworth 0:1b64a0cedc5d 42 int lil_arm; int big_arm; int claw_arm;
tashworth 0:1b64a0cedc5d 43 int claw_rotate; int claw_open;} Coord;
tashworth 0:1b64a0cedc5d 44
tashworth 0:1b64a0cedc5d 45 Coord Arm_Table[] =
tashworth 0:1b64a0cedc5d 46 {
tashworth 0:1b64a0cedc5d 47 // POSITION ODER:
tashworth 0:1b64a0cedc5d 48 // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
tashworth 0:1b64a0cedc5d 49
tashworth 2:4e082e4c255d 50 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 2:4e082e4c255d 51 {OIL_RIG1, 1500, 1400, 1900, 900, 900, 0, 0}, // point laser at oilrig2
Fairy_Paolina 3:587441455259 52 {STORE_POSITION, 2350, 1050, 700, 500, 2350, 0, 0}, // Shape Detect
tashworth 2:4e082e4c255d 53 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 2:4e082e4c255d 54 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 2:4e082e4c255d 55 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 2:4e082e4c255d 56 {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
tashworth 0:1b64a0cedc5d 57
tashworth 0:1b64a0cedc5d 58 };
tashworth 0:1b64a0cedc5d 59
tashworth 0:1b64a0cedc5d 60 int main() {
tashworth 0:1b64a0cedc5d 61
Fairy_Paolina 3:587441455259 62
tashworth 0:1b64a0cedc5d 63 /*****************
tashworth 0:1b64a0cedc5d 64 INITIALIZATIONS
tashworth 0:1b64a0cedc5d 65 *******************/
tashworth 2:4e082e4c255d 66 //pc.baud(115200);
tashworth 2:4e082e4c255d 67 //Laser Range Finder Initialization
tashworth 0:1b64a0cedc5d 68 //lrf_baudCalibration();
tashworth 2:4e082e4c255d 69
tashworth 2:4e082e4c255d 70 //Servo initialization
tashworth 0:1b64a0cedc5d 71 initServoDriver();
tashworth 2:4e082e4c255d 72 servoBegin(); // initiates servos to start position
tashworth 2:4e082e4c255d 73 ServoOutputDisable = 0;
tashworth 2:4e082e4c255d 74
tashworth 0:1b64a0cedc5d 75 while(1){
Fairy_Paolina 3:587441455259 76 int instr;
Fairy_Paolina 3:587441455259 77
Fairy_Paolina 3:587441455259 78 printf("Set Servo Position = 0, Shape Detect = 1, Oil Rig Detect = 2");
Fairy_Paolina 3:587441455259 79 while(!pc.readable());
Fairy_Paolina 3:587441455259 80 pc.scanf("%d", &instr);
Fairy_Paolina 3:587441455259 81
Fairy_Paolina 3:587441455259 82 if(instr == 0){// Servo Control
Fairy_Paolina 3:587441455259 83 while(!ServoTest());
Fairy_Paolina 3:587441455259 84 }
Fairy_Paolina 3:587441455259 85 else if( instr == 1){// shape detect
Fairy_Paolina 3:587441455259 86 //servoPosition(3);
Fairy_Paolina 3:587441455259 87
Fairy_Paolina 3:587441455259 88
Fairy_Paolina 3:587441455259 89 }
Fairy_Paolina 3:587441455259 90 else if( instr == 2){// oil rig detect
Fairy_Paolina 3:587441455259 91 servoPosition(2);
tashworth 2:4e082e4c255d 92
Fairy_Paolina 3:587441455259 93 }
Fairy_Paolina 3:587441455259 94
Fairy_Paolina 3:587441455259 95 }// End Main while
tashworth 0:1b64a0cedc5d 96 }
tashworth 0:1b64a0cedc5d 97
tashworth 0:1b64a0cedc5d 98 /**************************************************
tashworth 0:1b64a0cedc5d 99 * FIRST STAGE
tashworth 0:1b64a0cedc5d 100 *
tashworth 0:1b64a0cedc5d 101 * - DETERMINE OIL RIG ON FIRE
tashworth 0:1b64a0cedc5d 102 * - DETERMINE PATH
tashworth 0:1b64a0cedc5d 103 *
tashworth 0:1b64a0cedc5d 104 **************************************************/
tashworth 0:1b64a0cedc5d 105
tashworth 0:1b64a0cedc5d 106 //TODO: EXTEND ARM AND FACE OILRIGS
tashworth 0:1b64a0cedc5d 107
tashworth 0:1b64a0cedc5d 108 //OILRIG 1 DISTANCE READING
tashworth 0:1b64a0cedc5d 109
tashworth 0:1b64a0cedc5d 110 //TODO: ROTATE ARM TO NEXT OIL RIG
tashworth 0:1b64a0cedc5d 111
tashworth 0:1b64a0cedc5d 112 //OILRIG 2 DISTANCE READING
tashworth 0:1b64a0cedc5d 113
tashworth 0:1b64a0cedc5d 114 //ROTATE ARM TO NEXT OIL RIG
tashworth 0:1b64a0cedc5d 115
tashworth 0:1b64a0cedc5d 116 //OILRIG 3 DISTANCE READING
tashworth 0:1b64a0cedc5d 117
Fairy_Paolina 3:587441455259 118
tashworth 0:1b64a0cedc5d 119
tashworth 0:1b64a0cedc5d 120
tashworth 0:1b64a0cedc5d 121
tashworth 0:1b64a0cedc5d 122 /************
tashworth 0:1b64a0cedc5d 123
tashworth 0:1b64a0cedc5d 124 Servo Functions
tashworth 0:1b64a0cedc5d 125
tashworth 0:1b64a0cedc5d 126 **************/
tashworth 0:1b64a0cedc5d 127
tashworth 0:1b64a0cedc5d 128 void setServoPulse(uint8_t n, float pulse) {
tashworth 1:fe4a0b47ff25 129 float pulselength = 20000; // 20,000 us per second
Fairy_Paolina 3:587441455259 130 int i = currentPosition[n], pstart;
tashworth 1:fe4a0b47ff25 131 pc.printf("\ncurrent position = %d\n", currentPosition[n]);
Fairy_Paolina 3:587441455259 132 int pulse2, pulse1;
Fairy_Paolina 3:587441455259 133
Fairy_Paolina 3:587441455259 134 // Set when to start the pulse for the different servos
Fairy_Paolina 3:587441455259 135 // and normalize values
Fairy_Paolina 3:587441455259 136 if(n < 2){
Fairy_Paolina 3:587441455259 137 pstart = 0;// need to change this to match the different servos
Fairy_Paolina 3:587441455259 138 pulse1= pulse + 500;
Fairy_Paolina 3:587441455259 139 }
Fairy_Paolina 3:587441455259 140 else{
Fairy_Paolina 3:587441455259 141 pstart = 0;
Fairy_Paolina 3:587441455259 142 pulse1=pulse+500;
Fairy_Paolina 3:587441455259 143 }
Fairy_Paolina 3:587441455259 144
tashworth 1:fe4a0b47ff25 145 if(currentPosition[n] < pulse){
tashworth 1:fe4a0b47ff25 146 pc.printf("\ncurrent position < pulse\n");
Fairy_Paolina 3:587441455259 147 for(i; i < pulse1; i++){
tashworth 1:fe4a0b47ff25 148 pulse2 = 4094 * i / pulselength;
Fairy_Paolina 3:587441455259 149 pwm.setPWM(n, pstart, pulse2);
tashworth 1:fe4a0b47ff25 150 wait_ms(3);
tashworth 1:fe4a0b47ff25 151 }
tashworth 1:fe4a0b47ff25 152 } else if (currentPosition[n] > pulse) {
tashworth 1:fe4a0b47ff25 153 pc.printf("\ncurrent position > pulse\n");
Fairy_Paolina 3:587441455259 154 for(i; i > pulse1; i--){
tashworth 1:fe4a0b47ff25 155 pulse2 = 4094 * i / pulselength;
Fairy_Paolina 3:587441455259 156 pwm.setPWM(n, pstart, pulse2);
tashworth 1:fe4a0b47ff25 157 wait_ms(3);
tashworth 1:fe4a0b47ff25 158 }
tashworth 1:fe4a0b47ff25 159 }
tashworth 1:fe4a0b47ff25 160 currentPosition[n] = i;
tashworth 1:fe4a0b47ff25 161 pc.printf("\nending position = %d\n\n", i);
tashworth 0:1b64a0cedc5d 162 }
tashworth 0:1b64a0cedc5d 163
tashworth 0:1b64a0cedc5d 164 void initServoDriver(void) {
tashworth 0:1b64a0cedc5d 165 pwm.begin();
tashworth 0:1b64a0cedc5d 166 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 0:1b64a0cedc5d 167 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 0:1b64a0cedc5d 168 pwm.setI2Cfreq(400000); //400kHz
tashworth 1:fe4a0b47ff25 169
tashworth 0:1b64a0cedc5d 170 }
tashworth 0:1b64a0cedc5d 171
tashworth 1:fe4a0b47ff25 172 void servoBegin(void){
tashworth 1:fe4a0b47ff25 173 setServoPulseNo_delay(0, 900);
tashworth 1:fe4a0b47ff25 174 setServoPulseNo_delay(1, 500);
tashworth 1:fe4a0b47ff25 175 setServoPulseNo_delay(2, 600);
tashworth 1:fe4a0b47ff25 176 setServoPulseNo_delay(3, 2450);
tashworth 1:fe4a0b47ff25 177 setServoPulseNo_delay(4, 2450);
tashworth 1:fe4a0b47ff25 178 setServoPulseNo_delay(5, 0);
tashworth 1:fe4a0b47ff25 179 setServoPulseNo_delay(6, 0);
tashworth 1:fe4a0b47ff25 180 }
tashworth 0:1b64a0cedc5d 181
tashworth 1:fe4a0b47ff25 182 void setServoPulseNo_delay(uint8_t n, float pulse) {
tashworth 1:fe4a0b47ff25 183 float pulselength = 20000; // 20,000 us per second
tashworth 1:fe4a0b47ff25 184 currentPosition[n] = pulse;
tashworth 1:fe4a0b47ff25 185 pulse = 4094 * pulse / pulselength;
tashworth 1:fe4a0b47ff25 186 pwm.setPWM(n, 0, pulse);
tashworth 1:fe4a0b47ff25 187 }
tashworth 0:1b64a0cedc5d 188
tashworth 0:1b64a0cedc5d 189 void servoPosition(int set){
tashworth 0:1b64a0cedc5d 190 //moves to current position
tashworth 2:4e082e4c255d 191 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 1:fe4a0b47ff25 192 setServoPulse(1, Arm_Table[set].base_arm);
tashworth 0:1b64a0cedc5d 193 setServoPulse(2, Arm_Table[set].lil_arm);
tashworth 0:1b64a0cedc5d 194 setServoPulse(3, Arm_Table[set].big_arm);
tashworth 0:1b64a0cedc5d 195 setServoPulse(4, Arm_Table[set].claw_arm);
tashworth 0:1b64a0cedc5d 196 setServoPulse(5, Arm_Table[set].claw_rotate);
tashworth 0:1b64a0cedc5d 197 setServoPulse(6, Arm_Table[set].claw_open);
tashworth 0:1b64a0cedc5d 198 }
tashworth 0:1b64a0cedc5d 199
Fairy_Paolina 3:587441455259 200 int ServoTest(void){
Fairy_Paolina 3:587441455259 201 int num, delta=50; // Change Delta to change increment size
Fairy_Paolina 3:587441455259 202 static int move[7]={900, 500, 600, 2450, 2450, 0, 0};
Fairy_Paolina 3:587441455259 203
Fairy_Paolina 3:587441455259 204 if(pc.readable()){
Fairy_Paolina 3:587441455259 205 num=pc.getc();
Fairy_Paolina 3:587441455259 206
Fairy_Paolina 3:587441455259 207 if(num == 'a' || num == 'z'){
Fairy_Paolina 3:587441455259 208 if(move[0]>2650)move[0]=2700;
Fairy_Paolina 3:587441455259 209 else if(move[0] < 550) move[0]=500;
Fairy_Paolina 3:587441455259 210
Fairy_Paolina 3:587441455259 211 if(num=='a')setServoPulse(0, (move[0]+=delta));
Fairy_Paolina 3:587441455259 212 else setServoPulse(0, (move[0]-=delta));
Fairy_Paolina 3:587441455259 213 }
Fairy_Paolina 3:587441455259 214 else if(num == 's' || num == 'x'){
Fairy_Paolina 3:587441455259 215 if(move[1]> 2650)move[1]=2700;
Fairy_Paolina 3:587441455259 216 else if(move[1] < 550) move[1]=500;
Fairy_Paolina 3:587441455259 217
Fairy_Paolina 3:587441455259 218 if(num=='s')setServoPulse(1, (move[1]+=delta));
Fairy_Paolina 3:587441455259 219 else setServoPulse(1, (move[1]-=delta));
Fairy_Paolina 3:587441455259 220 }
Fairy_Paolina 3:587441455259 221 else if(num == 'd' || num == 'c'){
Fairy_Paolina 3:587441455259 222 if(move[2]> 2650)move[2]=2700;
Fairy_Paolina 3:587441455259 223 else if(move[2] < 550) move[2]=500;
Fairy_Paolina 3:587441455259 224
Fairy_Paolina 3:587441455259 225 if(num=='d')setServoPulse(2, (move[2]+=delta));
Fairy_Paolina 3:587441455259 226 else setServoPulse(2, (move[2]-=delta));
Fairy_Paolina 3:587441455259 227 }
Fairy_Paolina 3:587441455259 228 else if(num == 'f' || num == 'v'){
Fairy_Paolina 3:587441455259 229 if(move[3]> 2650)move[3]=2700;
Fairy_Paolina 3:587441455259 230 else if(move[3] < 550) move[3]=500;
Fairy_Paolina 3:587441455259 231
Fairy_Paolina 3:587441455259 232 if(num=='f')setServoPulse(3, (move[3]+=delta));
Fairy_Paolina 3:587441455259 233 else setServoPulse(3, (move[3]-=delta));
Fairy_Paolina 3:587441455259 234 }
Fairy_Paolina 3:587441455259 235 else if(num == 'g' || num == 'b'){
Fairy_Paolina 3:587441455259 236 if(move[4]> 2650)move[4]=2700;
Fairy_Paolina 3:587441455259 237 else if(move[4] < 550) move[4]=500;
Fairy_Paolina 3:587441455259 238
Fairy_Paolina 3:587441455259 239 if(num=='g')setServoPulse(4, (move[4]+=delta));
Fairy_Paolina 3:587441455259 240 else setServoPulse(4, (move[4]-=delta));
Fairy_Paolina 3:587441455259 241 }
Fairy_Paolina 3:587441455259 242 else if(num == 'h' || num == 'n'){
Fairy_Paolina 3:587441455259 243 if(move[5]> 2650)move[5]=2700;
Fairy_Paolina 3:587441455259 244 else if(move[5] < 550) move[5]=500;
Fairy_Paolina 3:587441455259 245
Fairy_Paolina 3:587441455259 246 if(num=='h')setServoPulse(5, (move[5]+=delta));
Fairy_Paolina 3:587441455259 247 else setServoPulse(5, (move[5]-=delta));
Fairy_Paolina 3:587441455259 248 }
Fairy_Paolina 3:587441455259 249 else if(num == 'j' || num == 'n'){
Fairy_Paolina 3:587441455259 250 if(move[6]> 2650)move[6]=2700;
Fairy_Paolina 3:587441455259 251 else if(move[6] < 550) move[6]=500;
Fairy_Paolina 3:587441455259 252
Fairy_Paolina 3:587441455259 253 if(num=='j')setServoPulse(6, (move[6]+=delta));
Fairy_Paolina 3:587441455259 254 else setServoPulse(6, (move[6]-=delta));
Fairy_Paolina 3:587441455259 255 }
Fairy_Paolina 3:587441455259 256 else if(num== 'e') return 1;
Fairy_Paolina 3:587441455259 257
Fairy_Paolina 3:587441455259 258 pc.printf("0\t 1\t 2\t 3 \t4 \t 5 \t 6\t type e to end\n\r");
Fairy_Paolina 3:587441455259 259 pc.printf("%d\t %d\t %d\t %d \t%d \t %d \t %d\n\r",move[0],move[1], move[2], move[3],move[4],move[5],move[6]);
Fairy_Paolina 3:587441455259 260 }
Fairy_Paolina 3:587441455259 261 return 0;
Fairy_Paolina 3:587441455259 262 }
tashworth 0:1b64a0cedc5d 263