![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TJPS
Dependencies: Adafruit-PWM-Servo-Driver mbed
Fork of Adafruit-PWM-Servo-Driver_sample by
main.cpp
- Committer:
- Fairy_Paolina
- Date:
- 2014-02-24
- Revision:
- 2:a94e3ade9632
- Parent:
- 0:4323102e4255
- Child:
- 3:ff08522e6416
File content as of revision 2:a94e3ade9632:
#include "mbed.h" #include "Adafruit_PWMServoDriver.h" Adafruit_PWMServoDriver pwm(p28, p27); Serial pc(USBTX, USBRX); int num, input; typedef struct { int Arm1; int Arm2; int Arm3; int Arm4; int Arm5; } Coord; Coord Arm_Table[] = { //Arm1 Arm2 Arm3 Arm4 Arm5 //Claw Base {0, 1700, 1650, 1650, 0},// straight up {0, 2700, 600, 600, 0}//storing position }; void setServoPulse(uint8_t n, float pulse) { static int prev[6]; int inc, loop, i, value, set; float pulselength = 10000; // 10,000 us per second if(pulse>=500 && pulse<=2700){ inc=abs(pulse-prev[n]); if(inc <500){ inc=inc/16; loop=16; } else if(inc< 1000){ inc=inc/32; loop=32; } else{ inc= inc/64; loop=64; } value= prev[n] + inc; for(i=0; i<loop; i++){ set = 4094 * value / pulselength; pwm.setPWM(n, 0, set); if( prev[n] < pulse)value+= inc; else value-= inc; wait_ms(15); } prev[n]= pulse; pulse = 4094 * pulse / pulselength; pwm.setPWM(n, 0, pulse); } } 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); } void initServoDriver() { pwm.begin(); //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale(). pwm.setPrescale(64); //This value is decided for 10ms interval. pwm.setI2Cfreq(400000); //400kHz } int main() { //pwm.i2c_probe(); initServoDriver(); setPosition(1); setPosition(0); setPosition(1); while(1){ if(pc.readable()){ pc.scanf("%d %d", &num, &input); setServoPulse(num, input); } } }