Diff: main.cpp

Revision:

12:486068800862

Parent:

6:fe9767a50891

diff -r a8385ca0a275 -r 486068800862 main.cpp
--- a/main.cpp	Thu Sep 07 20:17:26 2017 +0900
+++ b/main.cpp	Mon Sep 11 20:31:59 2017 +0900
@@ -1,19 +1,134 @@
 #include "mbed.h"
 #include "pin_config.h"
-#include "bot.h"
+#include "wheel_unit.h"
+#include "slider.h"
+#include "PID_controller.h"
+#include "controller.h"
+
+DigitalOut powerSwitch(MDstop);
+DigitalOut RS485Controller(PWM1);
+Serial RS485(MDTX, MDRX, RS485_BAUD);
+
+WheelUnit quadOmni(&RS485Controller, &RS485);
+Slider slider(&RS485Controller, &RS485);
+PIDC pid(HMCsda, HMCscl, KC, TI, TD, INTERVAL);
+Controller pad(XBee1TX, XBee1RX, ADDR);
+ikarashiMDC fukuda(&RS485Controller, 1, 1, SM, &RS485);
+ikarashiMDC crab(&RS485Controller, 1, 2, SM, &RS485);
+ikarashiMDC joint(&RS485Controller, 1, 3, SM, &RS485);
+
+volatile bool receiveSuccessed;
+
+Serial pc(USBTX, USBRX, 115200);
+DigitalOut debugLED[3] = {
+    DigitalOut(DebugLED3),
+    DigitalOut(DebugLED4),
+    DigitalOut(DebugLED5)
+};
+
+void getpad()
+{
+    while(true) {
+        receiveSuccessed = pad.receiveState();
+        pc.printf("%d\n\r", receiveSuccessed);
+        wait(0.0);
+        debugLED[0] = receiveSuccessed;
+    }
+}
+
+void getPID()
+{
+    debugLED[1] = receiveSuccessed;
+    if(receiveSuccessed) {
+        pid.confirm();
+    }
+}
+
+void allStop()
+{
+    quadOmni.moveXY(0, 0, 0);
+    slider.slide(0);
+    fukuda.setSpeed(0);
+    crab.setSpeed(0);
+    joint.setSpeed(0);
+}
 
-Bot KANI;
-Serial pc(USBTX, USBRX, 115200);
+void moveWheels()
+{
+    if(pad.getNorm(1) > 0.5) {
+        pid.PID::setSetPoint((pad.getRadian(1) - PI / 2) * (180.0 / PI));
+    }
+    quadOmni.moveXY(
+        pad.getStick(0) / 2.0,
+        -pad.getStick(1) /2.0,
+        pid.getCalculationResult()
+    );
+}
+
+void moveMach()
+{
+//        if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
+//        if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
+//        if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
+//
+//        if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
+//        if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
+//        if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
+//
+        if(!pad.getButton1(5)) fukuda.setSpeed(0.6);
+        if(!pad.getButton1(6)) fukuda.setSpeed(-0.6);
+        if(pad.getButton1(5) && pad.getButton1(6)) fukuda.setSpeed(-0.0);
+
+//
+//        if(!pad.getButton1(2)) {
+//            motor.destroy(DESTROY_MAX_SPEED);
+//        } else {
+//            motor.destroy(0);
+//        }
+//
+       if(!pad.getButton2(1)) slider.release();
+       if(!pad.getButton2(3)) powerSwitch = 0;
+}
+
+void moveMotors()
+{
+    if(receiveSuccessed) {
+        moveWheels();
+        moveMach();
+        debugLED[2] = receiveSuccessed;
+    } else {
+        allStop();
+    }
+}
+
+void LChika()
+{
+    for(int i = 0; i < 3; i++) {
+        debugLED[i] = true;
+        wait(0.1);
+        debugLED[i] = false;
+    }
+}
 
 int main()
 {
-    pc.printf("const\n\r");
-    while(1) {
-        KANI.confirmAll();
-        // KANI.controllDrive();
-        KANI.controllDrive2();
-        KANI.controllMech();
-        KANI.calibrate();
+    LChika();
+    fukuda.braking = true;
+    crab.braking = true;
+    joint.braking = true;
+    receiveSuccessed = false;
+    powerSwitch = true;
+    LChika();
+
+    int error = 0;
+    while(true) {
         wait(INTERVAL);
+        getpad();
+        getPID();
+        moveMotors();
+
+        error++;
+        if(receiveSuccessed) error = 0;
+        if(error > 10) powerSwitch = 0;
     }
 }