Diff: main.cpp

Revision:

10:04f2a82cfd89

Parent:

9:6486f4b3ac50

Child:

12:1fec80ae8a2c

--- a/main.cpp	Tue Jul 11 09:39:40 2017 +0000
+++ b/main.cpp	Sun Jul 16 04:06:49 2017 +0000
@@ -9,6 +9,7 @@
 #include "pid.h"
 #include "limit.h"
 #include "accelerator.h"
+#include "encorder.h"
 
 #define pc_baud     460800
 #define sbdbt_tx    p13
@@ -22,13 +23,23 @@
 #define pi          3.141592
 #define n1_id       3
 #define n2_id       4
-#define n3_id       0
+#define n3_id       5
 #define yaw_Kp      0.01
 #define yaw_Ki      0.01
 #define yaw_Kd      0.01
-#define acceleration   5
+#define acceleration    25
+#define pin_cylinder_on     p17
+#define pin_cylinder_off    p18
+#define interrupt       p16
+#define encoder_cylinder_A       p25
+#define encoder_cylinder_B       p26
+#define enc_Kp      0.01
+#define enc_Ki      0.01
+#define enc_Kd      0.01
 
 DigitalOut led(LED1);
+DigitalOut sylinder_on(pin_cylinder_on);
+DigitalOut sylinder_off(pin_cylinder_off);
 Serial pc(USBTX,USBRX);
 RS422 rs422(rs422_tx, rs422_rx);
 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
@@ -40,11 +51,13 @@
 Accel v2;
 Accel v3;
 Accel v4;
-
+Encoder enc_cylinder(encoder_cylinder_A,encoder_cylinder_B);
 
 void setup();
 void output();
 void put_output();
+void cylinder_check();
+void boost();
 float yaw, target_yaw;
 
 int main()
@@ -65,13 +78,47 @@
     rs422.begin(rs422_baud);
     output_timer.attach(&output, output_period);
     yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
-    mecanum.setupdeg(bno055.getYawRad());
+    mecanum.setupdeg(bno055.getYawRad()+180.0);
     v1.setup(acceleration,output_period);
     v2.setup(acceleration,output_period);
     v3.setup(acceleration,output_period);
     v4.setup(acceleration,output_period);
+    enc_cylinder.setup(1200);
+    enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
 }
 
+void boost(){
+    if(sbdbt.sankaku) {
+        mecanum.boost_forward();
+    }
+    if(sbdbt.batu) {
+        mecanum.boost_back();
+    }
+    if(sbdbt.shikaku) {
+        mecanum.boost_left();
+    }
+    if(sbdbt.maru) {
+        mecanum.boost_right();
+    }
+}
+
+void cylinder_check()
+{
+    if(sbdbt.sankaku) {
+        sylinder_on = 1;
+    } else {
+        sylinder_on = 0;
+    }
+
+    if(sbdbt.shikaku) {
+        sylinder_off = 1;
+    } else {
+        sylinder_off = 0;
+    }
+}
+
+
+
 void put_output()
 {
     yaw = bno055.getYawRad();
@@ -84,13 +131,12 @@
 void output()
 {
     put_output();
+    //cylinder_check();
+    boost();
 
     static int counter;
     int id[nucleo_num] = {n1_id, n2_id, n3_id};
 
-    if(sbdbt.batu) {
-        mecanum.boost();
-    }
 
     switch (counter) {
         case 0:
@@ -102,7 +148,7 @@
             counter ++;
             break;
         case 2:
-            rs422.put(id[counter], 0.0, 0.0);
+            rs422.put(id[counter], sbdbt.right_y, 0.0);
             counter = 0;
             break;
         default: