Hybrid_ServoMotor.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 
00004 RawSerial pc(USBTX, USBRX);
00005 
00006 #define SERVO1_PIN p22
00007 
00008 #define MAX_PULSE 0.00245//[s]
00009 #define MIN_PULSE 0.00065//[s]
00010 Servo servo1(SERVO1_PIN, MAX_PULSE, MIN_PULSE);
00011 
00012 DigitalOut led1(LED1);
00013 DigitalOut led2(LED2);
00014 DigitalOut led3(LED3);
00015 DigitalOut led4(LED4);
00016 
00017 DigitalOut nicrom(p21);
00018 
00019 void angleChange(){
00020     const int BUFFER_SIZE = 16;
00021     static float angle = 0;
00022     static char buff[BUFFER_SIZE] = {'\0'};
00023     static int count = 0;
00024     static bool startFlag = false;
00025     
00026     char temp = pc.getc();   
00027     if(temp == 'u') nicrom = 1;
00028     else if(temp == 'd')    nicrom = 0;
00029     pc.printf("%c", temp);
00030     if(temp == '@'){
00031         startFlag = true;
00032         memset(buff, '\0', BUFFER_SIZE);
00033         
00034         return;
00035     }
00036     if(startFlag){
00037         
00038         if(temp == '\r'){
00039             buff[count] = '\0';
00040             startFlag = false;
00041             angle = (float)atof(buff);
00042             servo1 = angle;
00043             pc.printf("\r\nangle : %3.2f [degree]\r\n", servo1());
00044             pc.printf("Please send like @***.*<CR><LF>\r\n\r\n");
00045             pc.printf("---------------------------------------\r\n");
00046             /*
00047             if(angle < -90.0)   angle = -90.0f;
00048             else if(angle > 90.0)  angle = 90.0f;
00049             servo1.position(angle);
00050             pc.printf("angle : %3.0f [degree]\r\n", angle);*/
00051                
00052             memset(buff, '\0', BUFFER_SIZE);
00053             count = 0;
00054             return;
00055         }   
00056         buff[count] = temp;
00057         count++;
00058         if(count > BUFFER_SIZE - 1){//null文字分空けておく
00059             startFlag = false;
00060             memset(buff, '\0', BUFFER_SIZE);
00061             count = 0;
00062             pc.printf("\r");
00063             pc.printf("\033[K");
00064             return; 
00065         }
00066         
00067     }
00068 }
00069 
00070 void ledDriver(unsigned char bin){
00071     unsigned char cmd = bin & 0b00000001;
00072     if(cmd) led1 = 1;
00073     else    led1 = 0;
00074     cmd = bin & 0b00000010;
00075     if(cmd) led2 = 1;
00076     else    led2 = 0;            
00077     cmd = bin & 0b00000100;
00078     if(cmd) led3 = 1;
00079     else    led3 = 0;
00080     cmd = bin & 0b00001000;
00081     if(cmd) led4 = 1;
00082     else    led4 = 0;
00083 }
00084 
00085 int main() {
00086     
00087     wait(3.0);
00088     
00089     pc.baud(115200);
00090     pc.attach(&angleChange, Serial::RxIrq);
00091     //char count = 0b00000001;
00092     pc.printf("\r\nStart boot phase:\r\n");
00093     for(int i = 0; i < 100; i++){
00094         pc.printf("\033[K");
00095         pc.printf("Loading... : %3d [%%]\r", i);
00096         wait(0.01);   
00097     }
00098     pc.printf("Loading... : %3d [%%]\r\n", 100);
00099     pc.printf("  -> OK!\r\n");
00100     wait(1.0);
00101     pc.printf("Initialize...\r\n");
00102     for(int i = 0;i < 20; i++){
00103         ledDriver(0b00001111);
00104         wait(0.05);
00105         ledDriver(0b00000000);
00106         wait(0.05);   
00107     }
00108     servo1 = 0.0f;
00109     pc.printf("angle : %3.2f [degree]\r\n", servo1());
00110     pc.printf("  -> OK!\r\n");
00111     wait(0.5);
00112     
00113     pc.printf("End boot phase\r\n");
00114     wait(1.0);
00115     pc.printf("Start _Servo Driving System ver 1.5.1_\r\n");
00116     pc.printf("\r\nPlease send like @***.*<CR><LF>, ***.* = angle[degree]\r\n");
00117    
00118     while(1) {    
00119         ledDriver(0b00000001);
00120         wait(0.2);
00121         ledDriver(0b00000010);
00122         wait(0.2);
00123         ledDriver(0b00000100);
00124         wait(0.2);
00125         ledDriver(0b00001000);
00126         wait(0.2);
00127     }
00128 }