Diff: main.cpp

Revision:

2:4a8227afe47b

Parent:

1:57b4a0340e93

diff -r 57b4a0340e93 -r 4a8227afe47b main.cpp
--- a/main.cpp	Mon Oct 01 12:05:13 2018 +0000
+++ b/main.cpp	Mon Oct 01 21:19:50 2018 +0900
@@ -1,4 +1,111 @@
-#include "mbed.h"
-
-int main(){
-}
\ No newline at end of file
+
+#include"wheel_unit.h"
+#include"mbed.h"
+
+wheel_unit wheel_unit;
+int main(){
+  while(true){
+    wheel_unit.main_do();
+  }
+}
+/*
+#include"PS3.h"
+#include"mbed.h"
+#include"pin_config.h"
+#include"omni_wheel.h"
+#include"wheel.h"
+#include"ikarashiMDC.h"
+#include"SerialMultiByte.h"
+#include"PID.h"
+#define PI 3.141592
+
+SerialMultiByte sboard(SERIAL_TX,SERIAL_RX);
+PS3 ps3(PS3_TX,PS3_RX);
+PID pid(6.0,3.0,0.000005,0.01);
+Serial pc(USBTX,USBRX,115200);
+OmniWheel omni(4);
+Serial serial(MDC_TX,MDC_RX,115200);
+DigitalOut serialcontrol(D10);
+ikarashiMDC motor[]{
+    ikarashiMDC(&serialcontrol,1,0,SM,&serial),
+    ikarashiMDC(&serialcontrol,1,1,SM,&serial),
+    ikarashiMDC(&serialcontrol,1,2,SM,&serial),
+    ikarashiMDC(&serialcontrol,1,3,SM,&serial),
+};
+uint8_t rx_data[10];
+int b[12];
+int stick[4];
+int trigger[2];
+int i,angle,default_angle;
+double spin;
+int main(){
+  pid.setInputLimits(-18000,18000);
+  pid.setOutputLimits(-1.0,1.0);
+  pid.setBias(0);
+  pid.setMode(1);
+  sboard.baud(115200);
+  sboard.setHeaders('H','Z');
+  sboard.startReceive(2);
+  sboard.getData(rx_data);
+  default_angle = ((rx_data[0] << 8) | rx_data[1]);
+  if(rx_data[0] >= 128)
+    default_angle = ((default_angle - 32768) * -1);
+  omni.wheel[0].setRadian(PI / 4.0 * 1.0 );
+  omni.wheel[1].setRadian(PI / 4.0 * 7.0 );
+  omni.wheel[2].setRadian(PI / 4.0 * 5.0 );
+  omni.wheel[3].setRadian(PI / 4.0 * 3.0 );
+  while(1){
+    sboard.getData(rx_data);
+    angle = ((rx_data[0] << 8) | rx_data[1]);
+    if(rx_data[0] >= 128)
+      angle = ((angle - 32768) * -1);
+    if(b[5] == 1){
+      sboard.getData(rx_data);
+      default_angle = ((rx_data[0] << 8) | rx_data[1]);
+      if(rx_data[0] >= 128)
+      default_angle = ((default_angle - 32768) * -1);
+      pid.reset();
+    }
+    angle = -1*(angle - default_angle);
+    pc.printf("sb*%d/%d/%d/%d/",angle,rx_data[0],rx_data[1],angle);
+    pid.setSetPoint(0.0);
+    pid.setProcessValue(double(angle));
+    double spin_power = pid.compute();
+    pc.printf("sp1*%f",spin_power);
+    for(i = 0;i < 12;i++){
+      b[i] = ps3.getButton(i);
+      pc.printf("%d-",b[i]);
+    }
+    for(i = 0;i < 4;i++){
+      stick[i] = ps3.getStick(i);
+      pc.printf("%d--",stick[i]);
+    }
+    for(i = 0;i < 2;i++){
+      trigger[i] = ps3.getTrigger(i);
+      pc.printf("%d--",trigger[i]);
+    }
+    double X = (double)((stick[0]/255.0 - 0.5)*2.0);
+    double Y = (double)((stick[1]/255.0 - 0.5)*2.0);
+    if(b[0])Y=0.3;
+    if(b[1])Y=-0.3;
+    if(b[2])X=0.3;
+    if(b[3])X=-0.3;
+    if(X <=0.2&&X>=-0.2)X = 0.0;
+    if(Y <=0.2&&Y>=-0.2)Y = 0.0;
+    if(((trigger[0]-trigger[1])/255.0) == 0 && b[4] == 1){
+      spin = spin_power;
+    }
+    else{
+      spin = (double)((trigger[0]-trigger[1])/255.0);
+    }
+    omni.computeXY(Y,-X,spin);
+    for(int i = 0; i < 4; i++) {
+      motor[i].setSpeed(omni.wheel[i]);
+      pc.printf("%f//",omni.wheel[i].getOutput());
+    }
+    stick[0] = char(stick[0]);
+    pc.printf("%.3f-%.3f",X,Y);
+    pc.printf("\n\r");
+  }
+}
+*/