PQ_Hybrid_Electrical_Equipment_Team
シリアルモニタに @角度<CR><LF> と入力すると、その角度にサーボが回るプログラムです。
Revision 0:2d7d98ccddfa, committed 2017-01-18
- Comitter:
- Gaku0606
- Date:
- Wed Jan 18 22:12:15 2017 +0000
- Commit message:
- ????????@??<CR><LF> ??????????????????????????
Changed in this revision
diff -r 000000000000 -r 2d7d98ccddfa Hybrid_ServoMotor.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Hybrid_ServoMotor.cpp Wed Jan 18 22:12:15 2017 +0000
@@ -0,0 +1,128 @@
+#include "mbed.h"
+#include "Servo.h"
+
+RawSerial pc(USBTX, USBRX);
+
+#define SERVO1_PIN p22
+
+#define MAX_PULSE 0.00245//[s]
+#define MIN_PULSE 0.00065//[s]
+Servo servo1(SERVO1_PIN, MAX_PULSE, MIN_PULSE);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+DigitalOut nicrom(p21);
+
+void angleChange(){
+ const int BUFFER_SIZE = 16;
+ static float angle = 0;
+ static char buff[BUFFER_SIZE] = {'\0'};
+ static int count = 0;
+ static bool startFlag = false;
+
+ char temp = pc.getc();
+ if(temp == 'u') nicrom = 1;
+ else if(temp == 'd') nicrom = 0;
+ pc.printf("%c", temp);
+ if(temp == '@'){
+ startFlag = true;
+ memset(buff, '\0', BUFFER_SIZE);
+
+ return;
+ }
+ if(startFlag){
+
+ if(temp == '\r'){
+ buff[count] = '\0';
+ startFlag = false;
+ angle = (float)atof(buff);
+ servo1 = angle;
+ pc.printf("\r\nangle : %3.2f [degree]\r\n", servo1());
+ pc.printf("Please send like @***.*<CR><LF>\r\n\r\n");
+ pc.printf("---------------------------------------\r\n");
+ /*
+ if(angle < -90.0) angle = -90.0f;
+ else if(angle > 90.0) angle = 90.0f;
+ servo1.position(angle);
+ pc.printf("angle : %3.0f [degree]\r\n", angle);*/
+
+ memset(buff, '\0', BUFFER_SIZE);
+ count = 0;
+ return;
+ }
+ buff[count] = temp;
+ count++;
+ if(count > BUFFER_SIZE - 1){//null文字分空けておく
+ startFlag = false;
+ memset(buff, '\0', BUFFER_SIZE);
+ count = 0;
+ pc.printf("\r");
+ pc.printf("\033[K");
+ return;
+ }
+
+ }
+}
+
+void ledDriver(unsigned char bin){
+ unsigned char cmd = bin & 0b00000001;
+ if(cmd) led1 = 1;
+ else led1 = 0;
+ cmd = bin & 0b00000010;
+ if(cmd) led2 = 1;
+ else led2 = 0;
+ cmd = bin & 0b00000100;
+ if(cmd) led3 = 1;
+ else led3 = 0;
+ cmd = bin & 0b00001000;
+ if(cmd) led4 = 1;
+ else led4 = 0;
+}
+
+int main() {
+
+ wait(3.0);
+
+ pc.baud(115200);
+ pc.attach(&angleChange, Serial::RxIrq);
+ //char count = 0b00000001;
+ pc.printf("\r\nStart boot phase:\r\n");
+ for(int i = 0; i < 100; i++){
+ pc.printf("\033[K");
+ pc.printf("Loading... : %3d [%%]\r", i);
+ wait(0.01);
+ }
+ pc.printf("Loading... : %3d [%%]\r\n", 100);
+ pc.printf(" -> OK!\r\n");
+ wait(1.0);
+ pc.printf("Initialize...\r\n");
+ for(int i = 0;i < 20; i++){
+ ledDriver(0b00001111);
+ wait(0.05);
+ ledDriver(0b00000000);
+ wait(0.05);
+ }
+ servo1 = 0.0f;
+ pc.printf("angle : %3.2f [degree]\r\n", servo1());
+ pc.printf(" -> OK!\r\n");
+ wait(0.5);
+
+ pc.printf("End boot phase\r\n");
+ wait(1.0);
+ pc.printf("Start _Servo Driving System ver 1.5.1_\r\n");
+ pc.printf("\r\nPlease send like @***.*<CR><LF>, ***.* = angle[degree]\r\n");
+
+ while(1) {
+ ledDriver(0b00000001);
+ wait(0.2);
+ ledDriver(0b00000010);
+ wait(0.2);
+ ledDriver(0b00000100);
+ wait(0.2);
+ ledDriver(0b00001000);
+ wait(0.2);
+ }
+}
diff -r 000000000000 -r 2d7d98ccddfa Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Wed Jan 18 22:12:15 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/PQ_Hybrid_Electrical_Equipment_Team/code/Servo/#7ba85d855519
diff -r 000000000000 -r 2d7d98ccddfa mbed-dev.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-dev.lib Wed Jan 18 22:12:15 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-dev/#fa9ff456f731