main.cpp

00001 /*PCA9685 library example code
00002  *test library performed with two servomotors, attached in channel 0 and 1,  that rotate to the right and to the left
00003  *servo in chanel 0: Tower Pro SG90
00004  *servo in chanel 1:Goteck GS9025 MG
00005  */ 
00006 #include"PCA9685.h"
00007 #include"mbed.h"
00008 
00009 
00010 
00011 PCA9685 pwm(D14,D15);
00012  
00013 void setServoPulse(uint8_t n, float pulse) {
00014     pwm.setPWM(n, 0, pulse);
00015 }
00016  
00017 void initServoDriver() {
00018     pwm.begin();
00019     pwm.setPrescale(121);     // set 20ms for generic servos
00020     pwm.frequencyI2C(400000); //400kHz fast I2C comunication
00021 }
00022  
00023 int main() {
00024 
00025     while(1){
00026     initServoDriver();
00027     setServoPulse(0, 308); //0 degree chanel 0
00028     setServoPulse(1, 495); //-90 degree chanel 1    
00029     wait(1);
00030     setServoPulse(0, 135); //90 degree chanel 0
00031     setServoPulse(1, 308); //0 degree chanel 1
00032     wait(1);
00033     setServoPulse(1, 135); //90 degree chanel 1 
00034     setServoPulse(0, 509); //0 degree chanel 1
00035     wait(1);           
00036     for (int mov = 495; mov > 135; mov--){
00037     setServoPulse(1, mov);
00038     wait(0.003);}
00039     for (int mov = 115; mov < 509; mov++){
00040     setServoPulse(0, mov);
00041     wait(0.003);    
00042     }           
00043     }
00044 }