Paolina Povolotskaya / Mbed 2 deprecated Servos

TJPS

Dependencies:   Adafruit-PWM-Servo-Driver mbed

Fork of Adafruit-PWM-Servo-Driver_sample by Shundo Kishi

main.cpp@2:a94e3ade9632, 2014-02-24 (annotated)

Committer:
Fairy_Paolina
Date:
Mon Feb 24 22:53:26 2014 +0000
Revision:
2:a94e3ade9632
Parent:
0:4323102e4255
Child:
3:ff08522e6416
tjps;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 0:4323102e4255 1 #include "mbed.h"
syundo0730 0:4323102e4255 2 #include "Adafruit_PWMServoDriver.h"
syundo0730 0:4323102e4255 3
Fairy_Paolina 2:a94e3ade9632 4 Adafruit_PWMServoDriver pwm(p28, p27);
Fairy_Paolina 2:a94e3ade9632 5 Serial pc(USBTX, USBRX);
Fairy_Paolina 2:a94e3ade9632 6 int num, input;
Fairy_Paolina 2:a94e3ade9632 7
Fairy_Paolina 2:a94e3ade9632 8
Fairy_Paolina 2:a94e3ade9632 9 typedef struct { int Arm1; int Arm2; int Arm3; int Arm4; int Arm5; } Coord;
Fairy_Paolina 2:a94e3ade9632 10
Fairy_Paolina 2:a94e3ade9632 11 Coord Arm_Table[] =
Fairy_Paolina 2:a94e3ade9632 12 {
Fairy_Paolina 2:a94e3ade9632 13 //Arm1 Arm2 Arm3 Arm4 Arm5
Fairy_Paolina 2:a94e3ade9632 14 //Claw Base
Fairy_Paolina 2:a94e3ade9632 15
Fairy_Paolina 2:a94e3ade9632 16 {0, 1700, 1650, 1650, 0},// straight up
Fairy_Paolina 2:a94e3ade9632 17 {0, 2700, 600, 600, 0}//storing position
Fairy_Paolina 2:a94e3ade9632 18
Fairy_Paolina 2:a94e3ade9632 19 };
Fairy_Paolina 2:a94e3ade9632 20
Fairy_Paolina 2:a94e3ade9632 21
syundo0730 0:4323102e4255 22
syundo0730 0:4323102e4255 23 void setServoPulse(uint8_t n, float pulse) {
Fairy_Paolina 2:a94e3ade9632 24 static int prev[6];
Fairy_Paolina 2:a94e3ade9632 25 int inc, loop, i, value, set;
syundo0730 0:4323102e4255 26 float pulselength = 10000; // 10,000 us per second
Fairy_Paolina 2:a94e3ade9632 27
Fairy_Paolina 2:a94e3ade9632 28 if(pulse>=500 && pulse<=2700){
Fairy_Paolina 2:a94e3ade9632 29 inc=abs(pulse-prev[n]);
Fairy_Paolina 2:a94e3ade9632 30
Fairy_Paolina 2:a94e3ade9632 31 if(inc <500){
Fairy_Paolina 2:a94e3ade9632 32 inc=inc/16;
Fairy_Paolina 2:a94e3ade9632 33 loop=16;
Fairy_Paolina 2:a94e3ade9632 34 }
Fairy_Paolina 2:a94e3ade9632 35 else if(inc< 1000){
Fairy_Paolina 2:a94e3ade9632 36 inc=inc/32;
Fairy_Paolina 2:a94e3ade9632 37 loop=32;
Fairy_Paolina 2:a94e3ade9632 38 }
Fairy_Paolina 2:a94e3ade9632 39 else{
Fairy_Paolina 2:a94e3ade9632 40 inc= inc/64;
Fairy_Paolina 2:a94e3ade9632 41 loop=64;
Fairy_Paolina 2:a94e3ade9632 42 }
Fairy_Paolina 2:a94e3ade9632 43
Fairy_Paolina 2:a94e3ade9632 44 value= prev[n] + inc;
Fairy_Paolina 2:a94e3ade9632 45
Fairy_Paolina 2:a94e3ade9632 46 for(i=0; i<loop; i++){
Fairy_Paolina 2:a94e3ade9632 47 set = 4094 * value / pulselength;
Fairy_Paolina 2:a94e3ade9632 48 pwm.setPWM(n, 0, set);
Fairy_Paolina 2:a94e3ade9632 49 if( prev[n] < pulse)value+= inc;
Fairy_Paolina 2:a94e3ade9632 50 else value-= inc;
Fairy_Paolina 2:a94e3ade9632 51 wait_ms(15);
Fairy_Paolina 2:a94e3ade9632 52 }
Fairy_Paolina 2:a94e3ade9632 53
Fairy_Paolina 2:a94e3ade9632 54
Fairy_Paolina 2:a94e3ade9632 55 prev[n]= pulse;
Fairy_Paolina 2:a94e3ade9632 56
Fairy_Paolina 2:a94e3ade9632 57 pulse = 4094 * pulse / pulselength;
Fairy_Paolina 2:a94e3ade9632 58 pwm.setPWM(n, 0, pulse);
Fairy_Paolina 2:a94e3ade9632 59 }
Fairy_Paolina 2:a94e3ade9632 60 }
Fairy_Paolina 2:a94e3ade9632 61
Fairy_Paolina 2:a94e3ade9632 62 void setPosition( int set ){
Fairy_Paolina 2:a94e3ade9632 63
Fairy_Paolina 2:a94e3ade9632 64 setServoPulse(1, Arm_Table[set].Arm1);
Fairy_Paolina 2:a94e3ade9632 65 setServoPulse(2, Arm_Table[set].Arm2);
Fairy_Paolina 2:a94e3ade9632 66 setServoPulse(3, Arm_Table[set].Arm3);
Fairy_Paolina 2:a94e3ade9632 67 setServoPulse(4, Arm_Table[set].Arm4);
Fairy_Paolina 2:a94e3ade9632 68 setServoPulse(5, Arm_Table[set].Arm5);
Fairy_Paolina 2:a94e3ade9632 69
syundo0730 0:4323102e4255 70 }
syundo0730 0:4323102e4255 71
syundo0730 0:4323102e4255 72 void initServoDriver() {
syundo0730 0:4323102e4255 73 pwm.begin();
syundo0730 0:4323102e4255 74 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
syundo0730 0:4323102e4255 75 pwm.setPrescale(64); //This value is decided for 10ms interval.
syundo0730 0:4323102e4255 76 pwm.setI2Cfreq(400000); //400kHz
syundo0730 0:4323102e4255 77 }
syundo0730 0:4323102e4255 78
syundo0730 0:4323102e4255 79 int main() {
syundo0730 0:4323102e4255 80 //pwm.i2c_probe();
syundo0730 0:4323102e4255 81 initServoDriver();
Fairy_Paolina 2:a94e3ade9632 82 setPosition(1);
Fairy_Paolina 2:a94e3ade9632 83 setPosition(0);
Fairy_Paolina 2:a94e3ade9632 84 setPosition(1);
syundo0730 0:4323102e4255 85
Fairy_Paolina 2:a94e3ade9632 86 while(1){
Fairy_Paolina 2:a94e3ade9632 87 if(pc.readable()){
Fairy_Paolina 2:a94e3ade9632 88 pc.scanf("%d %d", &num, &input);
Fairy_Paolina 2:a94e3ade9632 89 setServoPulse(num, input);
Fairy_Paolina 2:a94e3ade9632 90 }
Fairy_Paolina 2:a94e3ade9632 91 }
Fairy_Paolina 2:a94e3ade9632 92
Fairy_Paolina 2:a94e3ade9632 93
syundo0730 0:4323102e4255 94 }