![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
added normalization and use of keyboard. I tested it and it worked
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver mbed
Fork of theRobot by
Revision 4:116829a5ae3c, committed 2014-03-08
- Comitter:
- Fairy_Paolina
- Date:
- Sat Mar 08 23:23:35 2014 +0000
- Parent:
- 3:587441455259
- Commit message:
- revision 3/8;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Mar 06 00:12:56 2014 +0000 +++ b/main.cpp Sat Mar 08 23:23:35 2014 +0000 @@ -3,7 +3,7 @@ #include "ShapeDetect.h" Serial pc(USBTX,USBRX); -Adafruit_PWMServoDriver pwm(p9,p10); +Adafruit_PWMServoDriver pwm(p28,p27); DigitalOut ServoOutputDisable(p8); extern Serial lrf; @@ -47,9 +47,9 @@ // POSITION ODER: // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open - {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position - {OIL_RIG1, 1500, 1400, 1900, 900, 900, 0, 0}, // point laser at oilrig2 - {STORE_POSITION, 2350, 1050, 700, 500, 2350, 0, 0}, // Shape Detect + {STORE_POSITION, 46, 0, 0, 100, 96, 0, 0}, // storing position + {OIL_RIG1, 83, 75, 100, 33, 33, 0, 0}, // point laser at oilrig2 + {STORE_POSITION, 100, 17, 17, 0, 100, 0, 0}, // Shape Detect {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position @@ -75,7 +75,7 @@ while(1){ int instr; - printf("Set Servo Position = 0, Shape Detect = 1, Oil Rig Detect = 2"); + pc.printf("Set Servo Position = 0, Shape Detect = 1, Oil Rig Detect = 2"); while(!pc.readable()); pc.scanf("%d", &instr); @@ -128,37 +128,38 @@ void setServoPulse(uint8_t n, float pulse) { float pulselength = 20000; // 20,000 us per second int i = currentPosition[n], pstart; - pc.printf("\ncurrent position = %d\n", currentPosition[n]); - int pulse2, pulse1; + int pulse2, min=500, max=2700; // Set when to start the pulse for the different servos // and normalize values - if(n < 2){ + /* if(n < 2){ pstart = 0;// need to change this to match the different servos - pulse1= pulse + 500; + pulse1= pulse ; } else{ pstart = 0; - pulse1=pulse+500; - } + pulse1=pulse; + } */ + + pulse= ((pulse*(max-min))/100)+min; if(currentPosition[n] < pulse){ - pc.printf("\ncurrent position < pulse\n"); - for(i; i < pulse1; i++){ + pc.printf("\ncurrent position < pulse\n\r"); + for(i=currentPosition[n]; i < pulse; i++){ pulse2 = 4094 * i / pulselength; pwm.setPWM(n, pstart, pulse2); - wait_ms(3); + wait_ms(1); } } else if (currentPosition[n] > pulse) { - pc.printf("\ncurrent position > pulse\n"); - for(i; i > pulse1; i--){ + pc.printf("\ncurrent position > pulse\n\r"); + for(i=currentPosition[n]; i > pulse; i--){ pulse2 = 4094 * i / pulselength; pwm.setPWM(n, pstart, pulse2); - wait_ms(3); + wait_ms(1); } } currentPosition[n] = i; - pc.printf("\nending position = %d\n\n", i); + pc.printf("\nending position = %d\n\n\r", i); } void initServoDriver(void) { @@ -170,17 +171,20 @@ } void servoBegin(void){ - setServoPulseNo_delay(0, 900); - setServoPulseNo_delay(1, 500); - setServoPulseNo_delay(2, 600); - setServoPulseNo_delay(3, 2450); - setServoPulseNo_delay(4, 2450); + setServoPulseNo_delay(0, 46); + setServoPulseNo_delay(1, 0); + setServoPulseNo_delay(2, 0); + setServoPulseNo_delay(3, 100); + setServoPulseNo_delay(4, 96); setServoPulseNo_delay(5, 0); setServoPulseNo_delay(6, 0); } void setServoPulseNo_delay(uint8_t n, float pulse) { float pulselength = 20000; // 20,000 us per second + int min=500, max=2700; + + pulse= ((pulse*(max-min))/100)+min; currentPosition[n] = pulse; pulse = 4094 * pulse / pulselength; pwm.setPWM(n, 0, pulse); @@ -198,57 +202,57 @@ } int ServoTest(void){ - int num, delta=50; // Change Delta to change increment size - static int move[7]={900, 500, 600, 2450, 2450, 0, 0}; + int num, delta=1; // Change Delta to change increment size + static int move[7]={46, 0, 0, 100, 96, 0, 0}; if(pc.readable()){ num=pc.getc(); if(num == 'a' || num == 'z'){ - if(move[0]>2650)move[0]=2700; - else if(move[0] < 550) move[0]=500; + if(move[0]>=(100-delta))move[0]=(100-delta); + else if(move[0] <= (0+delta)) move[0]=delta; if(num=='a')setServoPulse(0, (move[0]+=delta)); else setServoPulse(0, (move[0]-=delta)); } else if(num == 's' || num == 'x'){ - if(move[1]> 2650)move[1]=2700; - else if(move[1] < 550) move[1]=500; + if(move[1]>=(100-delta))move[1]=(100-delta); + else if(move[1] <= (0+delta)) move[1]=delta; if(num=='s')setServoPulse(1, (move[1]+=delta)); else setServoPulse(1, (move[1]-=delta)); } else if(num == 'd' || num == 'c'){ - if(move[2]> 2650)move[2]=2700; - else if(move[2] < 550) move[2]=500; + if(move[2]>=(100-delta))move[2]=(100-delta); + else if(move[2] <= (0+delta)) move[2]=delta; if(num=='d')setServoPulse(2, (move[2]+=delta)); else setServoPulse(2, (move[2]-=delta)); } else if(num == 'f' || num == 'v'){ - if(move[3]> 2650)move[3]=2700; - else if(move[3] < 550) move[3]=500; + if(move[3]>=(100-delta))move[3]=(100-delta); + else if(move[3] <= (0+delta)) move[3]=delta; if(num=='f')setServoPulse(3, (move[3]+=delta)); else setServoPulse(3, (move[3]-=delta)); } else if(num == 'g' || num == 'b'){ - if(move[4]> 2650)move[4]=2700; - else if(move[4] < 550) move[4]=500; + if(move[4]>=(100-delta))move[4]=(100-delta); + else if(move[4] <= (0+delta)) move[4]=delta; if(num=='g')setServoPulse(4, (move[4]+=delta)); else setServoPulse(4, (move[4]-=delta)); } else if(num == 'h' || num == 'n'){ - if(move[5]> 2650)move[5]=2700; - else if(move[5] < 550) move[5]=500; + if(move[5]>=(100-delta))move[5]=(100-delta); + else if(move[5] <= (0+delta)) move[5]=delta; if(num=='h')setServoPulse(5, (move[5]+=delta)); else setServoPulse(5, (move[5]-=delta)); } else if(num == 'j' || num == 'n'){ - if(move[6]> 2650)move[6]=2700; - else if(move[6] < 550) move[6]=500; + if(move[6]>=(100-delta))move[6]=(100-delta); + else if(move[6] <= (0+delta)) move[6]=delta; if(num=='j')setServoPulse(6, (move[6]+=delta)); else setServoPulse(6, (move[6]-=delta));