Paolina Povolotskaya / Mbed 2 deprecated theRobot

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

main.cpp@4:116829a5ae3c, 2014-03-08 (annotated)

Committer:
Fairy_Paolina
Date:
Sat Mar 08 23:23:35 2014 +0000
Revision:
4:116829a5ae3c
Parent:
3:587441455259 
revision 3/8;

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);
Fairy_Paolina 4:116829a5ae3c 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
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
Fairy_Paolina 4:116829a5ae3c 50 {STORE_POSITION, 46, 0, 0, 100, 96, 0, 0}, // storing position
Fairy_Paolina 4:116829a5ae3c 51 {OIL_RIG1, 83, 75, 100, 33, 33, 0, 0}, // point laser at oilrig2
Fairy_Paolina 4:116829a5ae3c 52 {STORE_POSITION, 100, 17, 17, 0, 100, 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 4:116829a5ae3c 78 pc.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;
Fairy_Paolina 4:116829a5ae3c 131 int pulse2, min=500, max=2700;
Fairy_Paolina 3:587441455259 132
Fairy_Paolina 3:587441455259 133 // Set when to start the pulse for the different servos
Fairy_Paolina 3:587441455259 134 // and normalize values
Fairy_Paolina 4:116829a5ae3c 135 /* if(n < 2){
Fairy_Paolina 3:587441455259 136 pstart = 0;// need to change this to match the different servos
Fairy_Paolina 4:116829a5ae3c 137 pulse1= pulse ;
Fairy_Paolina 3:587441455259 138 }
Fairy_Paolina 3:587441455259 139 else{
Fairy_Paolina 3:587441455259 140 pstart = 0;
Fairy_Paolina 4:116829a5ae3c 141 pulse1=pulse;
Fairy_Paolina 4:116829a5ae3c 142 } */
Fairy_Paolina 4:116829a5ae3c 143
Fairy_Paolina 4:116829a5ae3c 144 pulse= ((pulse*(max-min))/100)+min;
Fairy_Paolina 3:587441455259 145
tashworth 1:fe4a0b47ff25 146 if(currentPosition[n] < pulse){
Fairy_Paolina 4:116829a5ae3c 147 pc.printf("\ncurrent position < pulse\n\r");
Fairy_Paolina 4:116829a5ae3c 148 for(i=currentPosition[n]; i < pulse; i++){
tashworth 1:fe4a0b47ff25 149 pulse2 = 4094 * i / pulselength;
Fairy_Paolina 3:587441455259 150 pwm.setPWM(n, pstart, pulse2);
Fairy_Paolina 4:116829a5ae3c 151 wait_ms(1);
tashworth 1:fe4a0b47ff25 152 }
tashworth 1:fe4a0b47ff25 153 } else if (currentPosition[n] > pulse) {
Fairy_Paolina 4:116829a5ae3c 154 pc.printf("\ncurrent position > pulse\n\r");
Fairy_Paolina 4:116829a5ae3c 155 for(i=currentPosition[n]; i > pulse; i--){
tashworth 1:fe4a0b47ff25 156 pulse2 = 4094 * i / pulselength;
Fairy_Paolina 3:587441455259 157 pwm.setPWM(n, pstart, pulse2);
Fairy_Paolina 4:116829a5ae3c 158 wait_ms(1);
tashworth 1:fe4a0b47ff25 159 }
tashworth 1:fe4a0b47ff25 160 }
tashworth 1:fe4a0b47ff25 161 currentPosition[n] = i;
Fairy_Paolina 4:116829a5ae3c 162 pc.printf("\nending position = %d\n\n\r", i);
tashworth 0:1b64a0cedc5d 163 }
tashworth 0:1b64a0cedc5d 164
tashworth 0:1b64a0cedc5d 165 void initServoDriver(void) {
tashworth 0:1b64a0cedc5d 166 pwm.begin();
tashworth 0:1b64a0cedc5d 167 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
tashworth 0:1b64a0cedc5d 168 pwm.setPrescale(140); //This value is decided for 20ms interval.
tashworth 0:1b64a0cedc5d 169 pwm.setI2Cfreq(400000); //400kHz
tashworth 1:fe4a0b47ff25 170
tashworth 0:1b64a0cedc5d 171 }
tashworth 0:1b64a0cedc5d 172
tashworth 1:fe4a0b47ff25 173 void servoBegin(void){
Fairy_Paolina 4:116829a5ae3c 174 setServoPulseNo_delay(0, 46);
Fairy_Paolina 4:116829a5ae3c 175 setServoPulseNo_delay(1, 0);
Fairy_Paolina 4:116829a5ae3c 176 setServoPulseNo_delay(2, 0);
Fairy_Paolina 4:116829a5ae3c 177 setServoPulseNo_delay(3, 100);
Fairy_Paolina 4:116829a5ae3c 178 setServoPulseNo_delay(4, 96);
tashworth 1:fe4a0b47ff25 179 setServoPulseNo_delay(5, 0);
tashworth 1:fe4a0b47ff25 180 setServoPulseNo_delay(6, 0);
tashworth 1:fe4a0b47ff25 181 }
tashworth 0:1b64a0cedc5d 182
tashworth 1:fe4a0b47ff25 183 void setServoPulseNo_delay(uint8_t n, float pulse) {
tashworth 1:fe4a0b47ff25 184 float pulselength = 20000; // 20,000 us per second
Fairy_Paolina 4:116829a5ae3c 185 int min=500, max=2700;
Fairy_Paolina 4:116829a5ae3c 186
Fairy_Paolina 4:116829a5ae3c 187 pulse= ((pulse*(max-min))/100)+min;
tashworth 1:fe4a0b47ff25 188 currentPosition[n] = pulse;
tashworth 1:fe4a0b47ff25 189 pulse = 4094 * pulse / pulselength;
tashworth 1:fe4a0b47ff25 190 pwm.setPWM(n, 0, pulse);
tashworth 1:fe4a0b47ff25 191 }
tashworth 0:1b64a0cedc5d 192
tashworth 0:1b64a0cedc5d 193 void servoPosition(int set){
tashworth 0:1b64a0cedc5d 194 //moves to current position
tashworth 2:4e082e4c255d 195 setServoPulse(0, Arm_Table[set].base_rotate);
tashworth 1:fe4a0b47ff25 196 setServoPulse(1, Arm_Table[set].base_arm);
tashworth 0:1b64a0cedc5d 197 setServoPulse(2, Arm_Table[set].lil_arm);
tashworth 0:1b64a0cedc5d 198 setServoPulse(3, Arm_Table[set].big_arm);
tashworth 0:1b64a0cedc5d 199 setServoPulse(4, Arm_Table[set].claw_arm);
tashworth 0:1b64a0cedc5d 200 setServoPulse(5, Arm_Table[set].claw_rotate);
tashworth 0:1b64a0cedc5d 201 setServoPulse(6, Arm_Table[set].claw_open);
tashworth 0:1b64a0cedc5d 202 }
tashworth 0:1b64a0cedc5d 203
Fairy_Paolina 3:587441455259 204 int ServoTest(void){
Fairy_Paolina 4:116829a5ae3c 205 int num, delta=1; // Change Delta to change increment size
Fairy_Paolina 4:116829a5ae3c 206 static int move[7]={46, 0, 0, 100, 96, 0, 0};
Fairy_Paolina 3:587441455259 207
Fairy_Paolina 3:587441455259 208 if(pc.readable()){
Fairy_Paolina 3:587441455259 209 num=pc.getc();
Fairy_Paolina 3:587441455259 210
Fairy_Paolina 3:587441455259 211 if(num == 'a' || num == 'z'){
Fairy_Paolina 4:116829a5ae3c 212 if(move[0]>=(100-delta))move[0]=(100-delta);
Fairy_Paolina 4:116829a5ae3c 213 else if(move[0] <= (0+delta)) move[0]=delta;
Fairy_Paolina 3:587441455259 214
Fairy_Paolina 3:587441455259 215 if(num=='a')setServoPulse(0, (move[0]+=delta));
Fairy_Paolina 3:587441455259 216 else setServoPulse(0, (move[0]-=delta));
Fairy_Paolina 3:587441455259 217 }
Fairy_Paolina 3:587441455259 218 else if(num == 's' || num == 'x'){
Fairy_Paolina 4:116829a5ae3c 219 if(move[1]>=(100-delta))move[1]=(100-delta);
Fairy_Paolina 4:116829a5ae3c 220 else if(move[1] <= (0+delta)) move[1]=delta;
Fairy_Paolina 3:587441455259 221
Fairy_Paolina 3:587441455259 222 if(num=='s')setServoPulse(1, (move[1]+=delta));
Fairy_Paolina 3:587441455259 223 else setServoPulse(1, (move[1]-=delta));
Fairy_Paolina 3:587441455259 224 }
Fairy_Paolina 3:587441455259 225 else if(num == 'd' || num == 'c'){
Fairy_Paolina 4:116829a5ae3c 226 if(move[2]>=(100-delta))move[2]=(100-delta);
Fairy_Paolina 4:116829a5ae3c 227 else if(move[2] <= (0+delta)) move[2]=delta;
Fairy_Paolina 3:587441455259 228
Fairy_Paolina 3:587441455259 229 if(num=='d')setServoPulse(2, (move[2]+=delta));
Fairy_Paolina 3:587441455259 230 else setServoPulse(2, (move[2]-=delta));
Fairy_Paolina 3:587441455259 231 }
Fairy_Paolina 3:587441455259 232 else if(num == 'f' || num == 'v'){
Fairy_Paolina 4:116829a5ae3c 233 if(move[3]>=(100-delta))move[3]=(100-delta);
Fairy_Paolina 4:116829a5ae3c 234 else if(move[3] <= (0+delta)) move[3]=delta;
Fairy_Paolina 3:587441455259 235
Fairy_Paolina 3:587441455259 236 if(num=='f')setServoPulse(3, (move[3]+=delta));
Fairy_Paolina 3:587441455259 237 else setServoPulse(3, (move[3]-=delta));
Fairy_Paolina 3:587441455259 238 }
Fairy_Paolina 3:587441455259 239 else if(num == 'g' || num == 'b'){
Fairy_Paolina 4:116829a5ae3c 240 if(move[4]>=(100-delta))move[4]=(100-delta);
Fairy_Paolina 4:116829a5ae3c 241 else if(move[4] <= (0+delta)) move[4]=delta;
Fairy_Paolina 3:587441455259 242
Fairy_Paolina 3:587441455259 243 if(num=='g')setServoPulse(4, (move[4]+=delta));
Fairy_Paolina 3:587441455259 244 else setServoPulse(4, (move[4]-=delta));
Fairy_Paolina 3:587441455259 245 }
Fairy_Paolina 3:587441455259 246 else if(num == 'h' || num == 'n'){
Fairy_Paolina 4:116829a5ae3c 247 if(move[5]>=(100-delta))move[5]=(100-delta);
Fairy_Paolina 4:116829a5ae3c 248 else if(move[5] <= (0+delta)) move[5]=delta;
Fairy_Paolina 3:587441455259 249
Fairy_Paolina 3:587441455259 250 if(num=='h')setServoPulse(5, (move[5]+=delta));
Fairy_Paolina 3:587441455259 251 else setServoPulse(5, (move[5]-=delta));
Fairy_Paolina 3:587441455259 252 }
Fairy_Paolina 3:587441455259 253 else if(num == 'j' || num == 'n'){
Fairy_Paolina 4:116829a5ae3c 254 if(move[6]>=(100-delta))move[6]=(100-delta);
Fairy_Paolina 4:116829a5ae3c 255 else if(move[6] <= (0+delta)) move[6]=delta;
Fairy_Paolina 3:587441455259 256
Fairy_Paolina 3:587441455259 257 if(num=='j')setServoPulse(6, (move[6]+=delta));
Fairy_Paolina 3:587441455259 258 else setServoPulse(6, (move[6]-=delta));
Fairy_Paolina 3:587441455259 259 }
Fairy_Paolina 3:587441455259 260 else if(num== 'e') return 1;
Fairy_Paolina 3:587441455259 261
Fairy_Paolina 3:587441455259 262 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 263 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 264 }
Fairy_Paolina 3:587441455259 265 return 0;
Fairy_Paolina 3:587441455259 266 }
tashworth 0:1b64a0cedc5d 267