![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TJPS
Dependencies: Adafruit-PWM-Servo-Driver mbed
Fork of Adafruit-PWM-Servo-Driver_sample by
Diff: main.cpp
- Revision:
- 3:ff08522e6416
- Parent:
- 2:a94e3ade9632
diff -r a94e3ade9632 -r ff08522e6416 main.cpp --- a/main.cpp Mon Feb 24 22:53:26 2014 +0000 +++ b/main.cpp Mon Mar 03 13:36:17 2014 +0000 @@ -1,44 +1,51 @@ #include "mbed.h" #include "Adafruit_PWMServoDriver.h" +#define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096) + Adafruit_PWMServoDriver pwm(p28, p27); Serial pc(USBTX, USBRX); -int num, input; +int input; +char num; -typedef struct { int Arm1; int Arm2; int Arm3; int Arm4; int Arm5; } Coord; - -Coord Arm_Table[] = +int Arm_Table[][7] = { - //Arm1 Arm2 Arm3 Arm4 Arm5 - //Claw Base - - {0, 1700, 1650, 1650, 0},// straight up - {0, 2700, 600, 600, 0}//storing position + //Arm0 Arm1 Arm2 Arm3 Arm4 Arm5 Arm 6 + //Base Claw + + {2500, 550, 600, 2700, 2700, 0, 0},// storing + {1300, 800, 1150, 2500, 2700, 0, 0},// straight up + {2350, 1050, 700, 500, 2350, 0, 0}// over tools }; void setServoPulse(uint8_t n, float pulse) { - static int prev[6]; + static int prev[7]={2500, 550, 600, 2700, 2700, 0, 0}; int inc, loop, i, value, set; float pulselength = 10000; // 10,000 us per second - if(pulse>=500 && pulse<=2700){ + if(pulse>=500 && pulse<=2700){ inc=abs(pulse-prev[n]); - if(inc <500){ - inc=inc/16; - loop=16; + if(inc <55){ + inc=inc/10; + loop=10; } - else if(inc< 1000){ + else if(inc <500){ inc=inc/32; loop=32; } + else if(inc< 1000){ + inc=inc/70; + loop=70; + } else{ - inc= inc/64; - loop=64; + inc= inc/100; + loop=100; } value= prev[n] + inc; @@ -60,12 +67,10 @@ } void setPosition( int set ){ - - setServoPulse(1, Arm_Table[set].Arm1); - setServoPulse(2, Arm_Table[set].Arm2); - setServoPulse(3, Arm_Table[set].Arm3); - setServoPulse(4, Arm_Table[set].Arm4); - setServoPulse(5, Arm_Table[set].Arm5); + int i; + + for(i=0; i<7; i++)setServoPulse(i, Arm_Table[set][i]); + } @@ -76,17 +81,82 @@ pwm.setI2Cfreq(400000); //400kHz } + + + int main() { + + int move[7]={2500, 550, 600, 2700, 2700, 0, 0}; + int i, delta=50; //pwm.i2c_probe(); initServoDriver(); - setPosition(1); + setPosition(0); + + setPosition(1);// laser range find setPosition(0); - setPosition(1); + + setPosition(2); //tool find + setPosition(0); + while(1){ if(pc.readable()){ - pc.scanf("%d %d", &num, &input); - setServoPulse(num, input); + num=pc.getc(); + + if(num == 'a' || num == 'z'){ + if(move[0]>2650)move[0]=2700; + else if(move[0] < 550) move[0]=500; + + 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(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(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(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(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(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(num=='j')setServoPulse(6, (move[6]+=delta)); + else setServoPulse(6, (move[6]-=delta)); + } + pc.printf("0\t 1\t 2\t 3 \t4 \t 5 \t 6\n\r"); + 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]); + //setServoPulse(num, input); } }