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@3:ff08522e6416, 2014-03-03 (annotated)

Committer:
Fairy_Paolina
Date:
Mon Mar 03 13:36:17 2014 +0000
Revision:
3:ff08522e6416
Parent:
2:a94e3ade9632 
update 1

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 3:ff08522e6416 4 #define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)
Fairy_Paolina 3:ff08522e6416 5 #define SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)
Fairy_Paolina 3:ff08522e6416 6
Fairy_Paolina 2:a94e3ade9632 7 Adafruit_PWMServoDriver pwm(p28, p27);
Fairy_Paolina 2:a94e3ade9632 8 Serial pc(USBTX, USBRX);
Fairy_Paolina 3:ff08522e6416 9 int input;
Fairy_Paolina 3:ff08522e6416 10 char num;
Fairy_Paolina 2:a94e3ade9632 11
Fairy_Paolina 2:a94e3ade9632 12
Fairy_Paolina 3:ff08522e6416 13 int Arm_Table[][7] =
Fairy_Paolina 2:a94e3ade9632 14 {
Fairy_Paolina 3:ff08522e6416 15 //Arm0 Arm1 Arm2 Arm3 Arm4 Arm5 Arm 6
Fairy_Paolina 3:ff08522e6416 16 //Base Claw
Fairy_Paolina 3:ff08522e6416 17
Fairy_Paolina 3:ff08522e6416 18 {2500, 550, 600, 2700, 2700, 0, 0},// storing
Fairy_Paolina 3:ff08522e6416 19 {1300, 800, 1150, 2500, 2700, 0, 0},// straight up
Fairy_Paolina 3:ff08522e6416 20 {2350, 1050, 700, 500, 2350, 0, 0}// over tools
Fairy_Paolina 2:a94e3ade9632 21
Fairy_Paolina 2:a94e3ade9632 22 };
Fairy_Paolina 2:a94e3ade9632 23
Fairy_Paolina 2:a94e3ade9632 24
syundo0730 0:4323102e4255 25
syundo0730 0:4323102e4255 26 void setServoPulse(uint8_t n, float pulse) {
Fairy_Paolina 3:ff08522e6416 27 static int prev[7]={2500, 550, 600, 2700, 2700, 0, 0};
Fairy_Paolina 2:a94e3ade9632 28 int inc, loop, i, value, set;
syundo0730 0:4323102e4255 29 float pulselength = 10000; // 10,000 us per second
Fairy_Paolina 2:a94e3ade9632 30
Fairy_Paolina 3:ff08522e6416 31 if(pulse>=500 && pulse<=2700){
Fairy_Paolina 2:a94e3ade9632 32 inc=abs(pulse-prev[n]);
Fairy_Paolina 2:a94e3ade9632 33
Fairy_Paolina 3:ff08522e6416 34 if(inc <55){
Fairy_Paolina 3:ff08522e6416 35 inc=inc/10;
Fairy_Paolina 3:ff08522e6416 36 loop=10;
Fairy_Paolina 2:a94e3ade9632 37 }
Fairy_Paolina 3:ff08522e6416 38 else if(inc <500){
Fairy_Paolina 2:a94e3ade9632 39 inc=inc/32;
Fairy_Paolina 2:a94e3ade9632 40 loop=32;
Fairy_Paolina 2:a94e3ade9632 41 }
Fairy_Paolina 3:ff08522e6416 42 else if(inc< 1000){
Fairy_Paolina 3:ff08522e6416 43 inc=inc/70;
Fairy_Paolina 3:ff08522e6416 44 loop=70;
Fairy_Paolina 3:ff08522e6416 45 }
Fairy_Paolina 2:a94e3ade9632 46 else{
Fairy_Paolina 3:ff08522e6416 47 inc= inc/100;
Fairy_Paolina 3:ff08522e6416 48 loop=100;
Fairy_Paolina 2:a94e3ade9632 49 }
Fairy_Paolina 2:a94e3ade9632 50
Fairy_Paolina 2:a94e3ade9632 51 value= prev[n] + inc;
Fairy_Paolina 2:a94e3ade9632 52
Fairy_Paolina 2:a94e3ade9632 53 for(i=0; i<loop; i++){
Fairy_Paolina 2:a94e3ade9632 54 set = 4094 * value / pulselength;
Fairy_Paolina 2:a94e3ade9632 55 pwm.setPWM(n, 0, set);
Fairy_Paolina 2:a94e3ade9632 56 if( prev[n] < pulse)value+= inc;
Fairy_Paolina 2:a94e3ade9632 57 else value-= inc;
Fairy_Paolina 2:a94e3ade9632 58 wait_ms(15);
Fairy_Paolina 2:a94e3ade9632 59 }
Fairy_Paolina 2:a94e3ade9632 60
Fairy_Paolina 2:a94e3ade9632 61
Fairy_Paolina 2:a94e3ade9632 62 prev[n]= pulse;
Fairy_Paolina 2:a94e3ade9632 63
Fairy_Paolina 2:a94e3ade9632 64 pulse = 4094 * pulse / pulselength;
Fairy_Paolina 2:a94e3ade9632 65 pwm.setPWM(n, 0, pulse);
Fairy_Paolina 2:a94e3ade9632 66 }
Fairy_Paolina 2:a94e3ade9632 67 }
Fairy_Paolina 2:a94e3ade9632 68
Fairy_Paolina 2:a94e3ade9632 69 void setPosition( int set ){
Fairy_Paolina 3:ff08522e6416 70 int i;
Fairy_Paolina 3:ff08522e6416 71
Fairy_Paolina 3:ff08522e6416 72 for(i=0; i<7; i++)setServoPulse(i, Arm_Table[set][i]);
Fairy_Paolina 3:ff08522e6416 73
Fairy_Paolina 2:a94e3ade9632 74
syundo0730 0:4323102e4255 75 }
syundo0730 0:4323102e4255 76
syundo0730 0:4323102e4255 77 void initServoDriver() {
syundo0730 0:4323102e4255 78 pwm.begin();
syundo0730 0:4323102e4255 79 //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale().
syundo0730 0:4323102e4255 80 pwm.setPrescale(64); //This value is decided for 10ms interval.
syundo0730 0:4323102e4255 81 pwm.setI2Cfreq(400000); //400kHz
syundo0730 0:4323102e4255 82 }
syundo0730 0:4323102e4255 83
Fairy_Paolina 3:ff08522e6416 84
Fairy_Paolina 3:ff08522e6416 85
Fairy_Paolina 3:ff08522e6416 86
syundo0730 0:4323102e4255 87 int main() {
Fairy_Paolina 3:ff08522e6416 88
Fairy_Paolina 3:ff08522e6416 89 int move[7]={2500, 550, 600, 2700, 2700, 0, 0};
Fairy_Paolina 3:ff08522e6416 90 int i, delta=50;
syundo0730 0:4323102e4255 91 //pwm.i2c_probe();
syundo0730 0:4323102e4255 92 initServoDriver();
Fairy_Paolina 3:ff08522e6416 93 setPosition(0);
Fairy_Paolina 3:ff08522e6416 94
Fairy_Paolina 3:ff08522e6416 95 setPosition(1);// laser range find
Fairy_Paolina 2:a94e3ade9632 96 setPosition(0);
Fairy_Paolina 3:ff08522e6416 97
Fairy_Paolina 3:ff08522e6416 98 setPosition(2); //tool find
Fairy_Paolina 3:ff08522e6416 99 setPosition(0);
Fairy_Paolina 3:ff08522e6416 100
syundo0730 0:4323102e4255 101
Fairy_Paolina 2:a94e3ade9632 102 while(1){
Fairy_Paolina 2:a94e3ade9632 103 if(pc.readable()){
Fairy_Paolina 3:ff08522e6416 104 num=pc.getc();
Fairy_Paolina 3:ff08522e6416 105
Fairy_Paolina 3:ff08522e6416 106 if(num == 'a' || num == 'z'){
Fairy_Paolina 3:ff08522e6416 107 if(move[0]>2650)move[0]=2700;
Fairy_Paolina 3:ff08522e6416 108 else if(move[0] < 550) move[0]=500;
Fairy_Paolina 3:ff08522e6416 109
Fairy_Paolina 3:ff08522e6416 110 if(num=='a'){
Fairy_Paolina 3:ff08522e6416 111 setServoPulse(0, (move[0]+=delta));
Fairy_Paolina 3:ff08522e6416 112 }
Fairy_Paolina 3:ff08522e6416 113 else setServoPulse(0, (move[0]-=delta));
Fairy_Paolina 3:ff08522e6416 114 }
Fairy_Paolina 3:ff08522e6416 115 else if(num == 's' || num == 'x'){
Fairy_Paolina 3:ff08522e6416 116 if(move[1]> 2650)move[1]=2700;
Fairy_Paolina 3:ff08522e6416 117 else if(move[1] < 550) move[1]=500;
Fairy_Paolina 3:ff08522e6416 118
Fairy_Paolina 3:ff08522e6416 119 if(num=='s')setServoPulse(1, (move[1]+=delta));
Fairy_Paolina 3:ff08522e6416 120 else setServoPulse(1, (move[1]-=delta));
Fairy_Paolina 3:ff08522e6416 121 }
Fairy_Paolina 3:ff08522e6416 122 else if(num == 'd' || num == 'c'){
Fairy_Paolina 3:ff08522e6416 123 if(move[2]> 2650)move[2]=2700;
Fairy_Paolina 3:ff08522e6416 124 else if(move[2] < 550) move[2]=500;
Fairy_Paolina 3:ff08522e6416 125
Fairy_Paolina 3:ff08522e6416 126 if(num=='d')setServoPulse(2, (move[2]+=delta));
Fairy_Paolina 3:ff08522e6416 127 else setServoPulse(2, (move[2]-=delta));
Fairy_Paolina 3:ff08522e6416 128 }
Fairy_Paolina 3:ff08522e6416 129 else if(num == 'f' || num == 'v'){
Fairy_Paolina 3:ff08522e6416 130 if(move[3]> 2650)move[3]=2700;
Fairy_Paolina 3:ff08522e6416 131 else if(move[3] < 550) move[3]=500;
Fairy_Paolina 3:ff08522e6416 132
Fairy_Paolina 3:ff08522e6416 133 if(num=='f')setServoPulse(3, (move[3]+=delta));
Fairy_Paolina 3:ff08522e6416 134 else setServoPulse(3, (move[3]-=delta));
Fairy_Paolina 3:ff08522e6416 135 }
Fairy_Paolina 3:ff08522e6416 136 else if(num == 'g' || num == 'b'){
Fairy_Paolina 3:ff08522e6416 137 if(move[4]> 2650)move[4]=2700;
Fairy_Paolina 3:ff08522e6416 138 else if(move[4] < 550) move[4]=500;
Fairy_Paolina 3:ff08522e6416 139
Fairy_Paolina 3:ff08522e6416 140 if(num=='g')setServoPulse(4, (move[4]+=delta));
Fairy_Paolina 3:ff08522e6416 141 else setServoPulse(4, (move[4]-=delta));
Fairy_Paolina 3:ff08522e6416 142 }
Fairy_Paolina 3:ff08522e6416 143 else if(num == 'h' || num == 'n'){
Fairy_Paolina 3:ff08522e6416 144 if(move[5]> 2650)move[5]=2700;
Fairy_Paolina 3:ff08522e6416 145 else if(move[5] < 550) move[5]=500;
Fairy_Paolina 3:ff08522e6416 146
Fairy_Paolina 3:ff08522e6416 147 if(num=='h')setServoPulse(5, (move[5]+=delta));
Fairy_Paolina 3:ff08522e6416 148 else setServoPulse(5, (move[5]-=delta));
Fairy_Paolina 3:ff08522e6416 149 }
Fairy_Paolina 3:ff08522e6416 150 else if(num == 'j' || num == 'n'){
Fairy_Paolina 3:ff08522e6416 151 if(move[6]> 2650)move[6]=2700;
Fairy_Paolina 3:ff08522e6416 152 else if(move[6] < 550) move[6]=500;
Fairy_Paolina 3:ff08522e6416 153
Fairy_Paolina 3:ff08522e6416 154 if(num=='j')setServoPulse(6, (move[6]+=delta));
Fairy_Paolina 3:ff08522e6416 155 else setServoPulse(6, (move[6]-=delta));
Fairy_Paolina 3:ff08522e6416 156 }
Fairy_Paolina 3:ff08522e6416 157 pc.printf("0\t 1\t 2\t 3 \t4 \t 5 \t 6\n\r");
Fairy_Paolina 3:ff08522e6416 158 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:ff08522e6416 159 //setServoPulse(num, input);
Fairy_Paolina 2:a94e3ade9632 160 }
Fairy_Paolina 2:a94e3ade9632 161 }
Fairy_Paolina 2:a94e3ade9632 162
Fairy_Paolina 2:a94e3ade9632 163
syundo0730 0:4323102e4255 164 }