Diff: main.cpp

Revision:

19:76a387e4bcf6

Parent:

18:ae4d97945b83

Child:

20:347daec6c9a3

Child:

22:ffded43c9fc6

--- a/main.cpp	Fri Aug 04 02:15:30 2017 +0000
+++ b/main.cpp	Fri Aug 04 07:07:04 2017 +0000
@@ -86,13 +86,13 @@
 void setup()
 {
     wait(1);
-//    bno055.begin();
+    bno055.begin();
     wait(1);
-//    bno055.firstRead();
+    bno055.firstRead();
     pc.baud(pc_baud);
     sbdbt.begin(sbdbt_baud);
     rs422.begin(rs422_baud);
-//    cylinder_origin();
+    cylinder_origin();
     output_timer.attach(&output, output_period);
     yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
     mecanum.setupdeg(bno055.getYawRad()+180.0);
@@ -126,11 +126,11 @@
             counter ++;
             break;
         case 2:
-            rs422.put(id[counter], 0.0, 0.0);
+            rs422.put(id[counter], sbdbt.right_y, 0.0);
             counter ++;
             break;
         case 3:
-            rs422.put(id[counter], sbdbt.right_y, ((float)sword_left.getState()));
+            rs422.put(id[counter], (sbdbt.sankaku*0.5-sbdbt.batu*0.5), ((float)sword_left.getState()));
             counter = 0;
             break;
         default:
@@ -150,6 +150,11 @@
     led1 = 0;
 }
 
+void cylinder_cal()
+{
+    cylinder.cyclic(sbdbt.up);
+}
+
 void boost(){
     if(sbdbt.sankaku) {
         mecanum.boost_forward();
@@ -165,11 +170,6 @@
     }
 }
 
-void cylinder_cal()
-{
-    cylinder.cyclic(sbdbt.down);
-}
-
 void motor_cal()
 {
     yaw = bno055.getYawRad();