ServoMove.cpp

00001 #include "ServoMove.h"
00002 PwmOut servo(PA_6);
00003 Serial pc1(USBTX, USBRX);
00004 
00005 void ServoConvert(double cnv)
00006 {
00007     if (cnv > 144) {
00008         servo.pulsewidth_us(1000); //0deg
00009         pc1.printf("cnv1 =%f\n\r",cnv);
00010     } else if(cnv >108  && cnv <144) {
00011         servo.pulsewidth_us(1100);
00012         pc1.printf("cnv2 =%f\n\r",cnv);
00013     } else if (cnv >72 && cnv <108) {
00014         servo.pulsewidth_us(1200);
00015         pc1.printf("cnv3 =%f\n\r",cnv);
00016     } else if (cnv > 36&& cnv <72) {
00017         servo.pulsewidth_us(1300);
00018         pc1.printf("cnv4 =%f\n\r",cnv);
00019     } else if(cnv > 0&& cnv <36) {
00020         servo.pulsewidth_us(1400);
00021         pc1.printf("cnv5 =%f\n\r",cnv);
00022     } else if(cnv > -36&& cnv <0) {
00023         servo.pulsewidth_us(1500);
00024         pc1.printf("cnv6 =%f\n\r",cnv);
00025     } else if(cnv > -72&& cnv <-36) {
00026         servo.pulsewidth_us(1600);
00027         pc1.printf("cnv7 =%f\n\r",cnv);
00028     } else if(cnv > -108&& cnv <-72) {
00029         servo.pulsewidth_us(1700);
00030         pc1.printf("cnv8 =%f\n\r",cnv);
00031     } else if(cnv > -144&& cnv <-108) {
00032         servo.pulsewidth_us(1800);
00033         pc1.printf("cnv9 =%f\n\r",cnv);
00034     } else if(cnv > -180&& cnv <-144) {
00035         servo.pulsewidth_us(1900);
00036         pc1.printf("cnv10 =%f\n\r",cnv);
00037     }
00038 }