Diff: main.cpp

Revision:

0:898471aea1af

Child:

1:93f6f64ac719

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 22 06:39:31 2019 +0000
@@ -0,0 +1,72 @@
+#include "mbed.h"           //wheel_test7
+#include "ikarashiMDC.h"    //sampleライブラリ+PID -> うひょひょひょ
+#include "omni_wheel.h"
+#include "sample02.h"
+#include "OmniPosition.h"
+#include "PID.h"
+
+PID angle(2.0,0.0,0.00001,0.01);
+OmniWheel omni(4);
+OmniPosition posi(PC_12, PD_2);
+Serial serial(PC_6, PC_7, 115200);
+DigitalOut serialcontrol(D10);
+RawSerial pc(USBTX, USBRX, 115200);
+DigitalOut sw1(PC_2);
+DigitalOut sw2(PC_3);
+
+ikarashiMDC motor[]=
+{
+    ikarashiMDC(&serialcontrol,0,0,SM,&serial),
+    ikarashiMDC(&serialcontrol,0,1,SM,&serial),
+    ikarashiMDC(&serialcontrol,0,2,SM,&serial),
+    ikarashiMDC(&serialcontrol,0,3,SM,&serial),
+};
+
+Sample sample(500, 1000, 0.4, 0.14);
+
+int main()
+{
+    sw1 = 1;
+    sw2 = 2;
+    int cnt=0;
+    motor[0].braking = true;
+    double valueX, valueY, value[4];
+    omni.wheel[0].setRadian(PI * 1 / 4);
+    omni.wheel[1].setRadian(PI * 3 / 4);
+    omni.wheel[2].setRadian(PI * 5 / 4);
+    omni.wheel[3].setRadian(PI * 7 / 4);
+    angle.setInputLimits(-3.14,3.14);
+    angle.setOutputLimits(-0.4,0.4);
+    angle.setBias(0);
+    angle.setMode(1);
+    angle.setSetPoint(0);
+    while(1){
+        
+        if (cnt==500) sample.target_xy(-3800, 0, posi.getX(), posi.getY());
+        if (cnt==1000) sample.target_xy(0, 0, posi.getX(), posi.getY());
+        if (cnt==1500) sample.target_xy(-2000, 0, posi.getX(), posi.getY());
+        if (cnt==2000) sample.target_xy(-2000, 2700, posi.getX(), posi.getY());
+        if (cnt==2500) sample.target_xy(-2000, 0, posi.getX(), posi.getY());
+        if (cnt==3000) sample.target_xy(0, 0, posi.getX(), posi.getY());
+        angle.setProcessValue(posi.getTheta());
+        double spin_power = -angle.compute();
+        sample.calculate(posi.getX(), posi.getY());
+        omni.computeXY(sample.getvalue_x(), sample.getvalue_y(), spin_power);
+//        omni.computeXY(0, 0, spin_power);
+        
+        for(int i = 0; i < 4; i++){
+            value[i] = omni.wheel[i];
+            pc.printf("%.2f || ",value[i]);
+        }
+        int a = sample.nowDis > (sample.accdis+sample.middledis);
+        pc.printf("x:%d || y:%d || θ:%f || vector:%f || now:%f || c:%f"
+        , posi.getX(), posi.getY(), posi.getTheta(), sample.vector, sample.nowDis, sample.counter);
+        
+        for(int i = 0; i < 4; i++) {
+            if (fabs(value[i]) < 0.05) value[i] = 0;
+            motor[i].setSpeed(value[i]);
+        }
+        cnt++;
+        pc.printf("||pid:%.2f\r\n",spin_power);
+    }
+}
\ No newline at end of file