main.cpp

00001 // Servo Motor controlled with Terminal
00002 
00003 #include "mbed.h"
00004 #include "Servo.h"
00005 
00006 Serial pc(USBTX, USBRX);
00007 Servo myservo(D6);
00008 float angolo;
00009 int getangle(){
00010     printf("\n insert angle value to sweep with servo motor \n");
00011 
00012     int numero = 0;
00013     int angle=0;
00014     int minus=0;
00015     int count = 0;
00016     char c=pc.getc();
00017     printf("\n angle: \t");
00018     int number[2]={0, 0};
00019     if(c=='-'){
00020         printf("-");
00021         minus=1;
00022         char c=pc.getc();
00023         angle = c -'0';
00024         printf("%d",angle);
00025             }
00026         else{
00027         angle = c -'0';
00028         printf("%d",angle);}      
00029     while((count !=1) && (c!='\r')) {
00030 
00031         number[count]=angle;
00032         count++;
00033         c=pc.getc();
00034         if (c!='\r'){
00035         angle = c -'0';
00036         printf("%d",angle);}
00037         }
00038     if (c!='\r'){
00039       number[count]=angle;}
00040       else{number[1]=angle;
00041       number[0]=0;}  
00042       numero = 10*number[0]+number[1];
00043       if(minus==0){
00044       return numero;}
00045       else{numero=-numero;
00046       return numero;}
00047       }  
00048 int main() {
00049         printf("\033[2J");
00050         printf("\n*****************************************\n");
00051         printf("*  Use key q to rotate at 90 degree     *\n");
00052         printf("*  Use key e to rotate at -90 degree    *\n");
00053         printf("*  Use key w to rotate at  0 degree     *\n");
00054         printf("*  Use key a to insert the sweep angle  *\n");
00055         printf("*Use key s to rotate 1 degree at a time *\n");
00056         printf("*Use key d to rotate -1 degree at a time*\n");
00057         printf("*****************************************\n");
00058         myservo.calibrate(0.0008,90);
00059         while(1) {
00060         char c = pc.getc();
00061         if((c == 's') && (angolo < 90.0)) {
00062             angolo += 1;
00063             myservo.position(angolo);
00064         }
00065         if((c == 'd') && (angolo > -90.0)) {
00066             angolo -= 1;
00067             myservo.position(angolo);
00068         } 
00069         if((c == 'q')) {
00070             angolo= 90;
00071             myservo.position(angolo);
00072         } 
00073         if((c == 'w')) {
00074             angolo= 0;
00075             myservo.position(angolo);
00076         }   
00077         if((c == 'e')) {
00078             angolo= -90;
00079             myservo.position(angolo);
00080         }   
00081         if ((c == 'a')){
00082 
00083                 angolo = getangle();
00084                 printf("\n Servo motor position:%f\n",angolo);
00085                 myservo.position(angolo);
00086         }  
00087         printf("\n Angle : %f", angolo);
00088 
00089     }
00090 
00091 
00092 }