Diff: main.cpp

Revision:

0:dde37cabc667

Child:

1:f52578243f96

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Nov 15 14:01:48 2020 +0000
@@ -0,0 +1,188 @@
+#include "mbed.h"
+
+/* ピン設定 */
+PwmOut Servo_1(PA_8);
+// PwmOut Servo_2(PA_0);  // us_tickerにてTIM2を利用中のため使用不可
+DigitalOut Servo_2_MANUAL(PA_0);
+PwmOut Servo_3(PA_6_ALT0);
+PwmOut Servo_4(PA_3);
+PwmOut Servo_5(PB_4);
+PwmOut Servo_6(PA_7_ALT1);
+Serial xbee(PA_9, PA_10);
+Serial pc(USBTX, USBRX);
+DigitalOut led[] = {
+    DigitalOut(PB_1),
+    DigitalOut(PF_0),
+    DigitalOut(PF_1),
+    };
+DigitalOut xbee_reset(PA_1);
+DigitalOut nucleo_led(PB_3);
+
+
+/* モジュールID */
+const int kModuleID = 1;
+
+
+/* サーボ */
+//                               MIN    MID     MAX
+double Servo_1_pulsewidth[3] = {0.001, 0.0015, 0.0020};
+double Servo_2_pulsewidth[3] = {0.001, 0.0015, 0.0020};
+double Servo_3_pulsewidth[3] = {0.001, 0.0015, 0.0020};
+double Servo_4_pulsewidth[3] = {0.001, 0.0015, 0.0020};
+double Servo_5_pulsewidth[3] = {0.001, 0.0015, 0.0020};
+double Servo_6_pulsewidth[3] = {0.001, 0.0015, 0.0020};
+
+Ticker tick_MANUAL;
+Timeout timo_MANUAL;
+double Servo_2_pw_current_MANUAL = 0.001; // 適当,いじる必要なし
+void Servo_2_off_MANUAL(){
+    Servo_2_MANUAL = 0;
+}
+void Servo_2_on_MANUAL(){
+    Servo_2_MANUAL = 1;
+    timo_MANUAL.attach(&Servo_2_off_MANUAL, Servo_2_pw_current_MANUAL);
+}
+void Servo_2_period_ms_MANUAL(int p_ms){
+    Servo_2_off_MANUAL();
+    tick_MANUAL.attach(&Servo_2_on_MANUAL, (double)p_ms / 1.0e3);
+}
+void Servo_2_pulsewidth_MANUAL(double p_ms){
+    Servo_2_pw_current_MANUAL = p_ms;
+}
+
+
+/* PM */
+void PM_receive(){
+    while(xbee.readable()){
+        pc.putc(xbee.getc());
+    }
+}
+void PM_init(){
+    xbee.baud(9600);
+    xbee_reset = 1;
+}
+void PM_on(){
+    xbee.printf("a");
+}
+void PM_off(){
+    xbee.printf(" ");
+}
+
+
+/* PCシリアル通信 */
+void SerialReceive(void)
+{
+    char message = pc.getc();
+    while(pc.readable())  // bufferを綺麗に
+        pc.getc();
+
+    switch (message) {
+        case 'q':
+            Servo_1.pulsewidth(Servo_1_pulsewidth[2]);
+            break;
+        case 'a':
+            Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
+            break;
+        case 'z':
+            Servo_1.pulsewidth(Servo_1_pulsewidth[0]);
+            break;
+        case 'w':
+            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[2]);
+            break;
+        case 's':
+            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
+            break;
+        case 'x':
+            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[0]);
+            break;
+        case 'e':
+            Servo_3.pulsewidth(Servo_3_pulsewidth[2]);
+            break;
+        case 'd':
+            Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
+            break;
+        case 'c':
+            Servo_3.pulsewidth(Servo_3_pulsewidth[0]);
+            break;
+        case 'r':
+            Servo_4.pulsewidth(Servo_4_pulsewidth[2]);
+            break;
+        case 'f':
+            Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
+            break;
+        case 'v':
+            Servo_4.pulsewidth(Servo_4_pulsewidth[0]);
+            break;
+        case 't':
+            Servo_5.pulsewidth(Servo_5_pulsewidth[2]);
+            break;
+        case 'g':
+            Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
+            break;
+        case 'b':
+            Servo_5.pulsewidth(Servo_5_pulsewidth[0]);
+            break;
+        case 'y':
+            Servo_6.pulsewidth(Servo_6_pulsewidth[2]);
+            break;
+        case 'h':
+            Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
+            break;
+        case 'n':
+            Servo_6.pulsewidth(Servo_6_pulsewidth[0]);
+            break;
+        case 'u':
+            PM_on();
+            break;
+        case 'j':
+            PM_off();
+            break;
+        default :
+            Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
+            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
+            Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
+            Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
+            Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
+            Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
+            PM_off();
+            break;
+    }
+    led[2] =~ led[2];
+}
+
+int main()
+{
+    Servo_1.period_ms(20);
+    Servo_2_period_ms_MANUAL(20);
+    Servo_3.period_ms(20);
+    Servo_4.period_ms(20);
+    Servo_5.period_ms(20);
+    Servo_6.period_ms(20);
+    Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
+    Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
+    Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
+    Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
+    Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
+    Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
+    PM_off();
+
+    pc.baud(9600);
+    pc.attach(SerialReceive,Serial::RxIrq);
+    PM_init();
+
+    // start motion
+    wait(1);
+
+    pc.printf("CM : %02d\n", kModuleID);
+
+    wait(1);
+    led[0] = 1;
+    
+    nucleo_led = 0;
+    while(1) {
+        PM_receive();
+        pc.printf("CM : %02d\n", kModuleID);
+        nucleo_led =~ nucleo_led;
+        wait(0.5);
+    }
+}
\ No newline at end of file