Diff: main.cpp

Revision:

1:9fd2d2f5184e

Parent:

0:305237ecbbe5

--- a/main.cpp	Wed Jul 22 13:23:02 2015 +0000
+++ b/main.cpp	Fri Jul 24 09:04:32 2015 +0000
@@ -1,5 +1,7 @@
 /*PCA9685 library example code
- *test library performed with two servomotor, attached in channel 0 and 1,  that rotate to the right and to the left
+ *test library performed with two servomotors, attached in channel 0 and 1,  that rotate to the right and to the left
+ *servo in chanel 0: Tower Pro SG90
+ *servo in chanel 1:Goteck GS9025 MG
  */ 
 #include"PCA9685.h"
 #include"mbed.h"
@@ -9,40 +11,34 @@
 PCA9685 pwm(D14,D15);
  
 void setServoPulse(uint8_t n, float pulse) {
-    float pulselength = 10000;   // 10,000 units per seconds
-    pulse = 4094 * pulse / pulselength;
     pwm.setPWM(n, 0, pulse);
 }
  
 void initServoDriver() {
     pwm.begin();
-    pwm.setPrescale(64);    //This value is decided for 10ms interval.
-    pwm.frequencyI2C(400000); //400kHz
+    pwm.setPrescale(121);     // set 20ms for generic servos
+    pwm.frequencyI2C(400000); //400kHz fast I2C comunication
 }
  
 int main() {
 
     while(1){
     initServoDriver();
-    wait(0.2);
-    setServoPulse(0, 2300);
-    setServoPulse(1, 500);    
-    wait(0.5);//delay necessary to perform the action
-    setServoPulse(0, 1350);
-    setServoPulse(1, 1350);
-    wait(0.5);
-    setServoPulse(0,550);
-    setServoPulse(1, 2250);
-    wait(0.5);
-    setServoPulse(0, 2300);
-    wait(2);
-    for (int mov = 550; mov < 2300; mov++){
+    setServoPulse(0, 308); //0 degree chanel 0
+    setServoPulse(1, 495); //-90 degree chanel 1    
+    wait(1);
+    setServoPulse(0, 135); //90 degree chanel 0
+    setServoPulse(1, 308); //0 degree chanel 1
+    wait(1);
+    setServoPulse(1, 135); //90 degree chanel 1 
+    setServoPulse(0, 509); //0 degree chanel 1
+    wait(1);           
+    for (int mov = 495; mov > 135; mov--){
+    setServoPulse(1, mov);
+    wait(0.003);}
+    for (int mov = 115; mov < 509; mov++){
     setServoPulse(0, mov);
-    wait(0.001); 
-    }  
-    for (int mov = 500; mov < 2200; mov++){
-    setServoPulse(1, mov);
-    wait(0.001); 
-    }     
+    wait(0.003);    
+    }           
     }
 }
\ No newline at end of file