シリアルモニタに @角度<CR><LF> と入力すると、その角度にサーボが回るプログラムです。
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 15 2022 18:50:15 by 1.7.2