Diff: main.cpp

Revision:

12:1fec80ae8a2c

Parent:

10:04f2a82cfd89

Child:

13:dfae731e239f

--- a/main.cpp	Sun Jul 16 04:06:49 2017 +0000
+++ b/main.cpp	Sun Jul 16 08:35:45 2017 +0000
@@ -10,6 +10,7 @@
 #include "limit.h"
 #include "accelerator.h"
 #include "encorder.h"
+#include "QEI.h"
 
 #define pc_baud     460800
 #define sbdbt_tx    p13
@@ -30,9 +31,9 @@
 #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 interrupt_pin       p23
+#define encoder_A       p25
+#define encoder_B       p26
 #define enc_Kp      0.01
 #define enc_Ki      0.01
 #define enc_Kd      0.01
@@ -40,6 +41,7 @@
 DigitalOut led(LED1);
 DigitalOut sylinder_on(pin_cylinder_on);
 DigitalOut sylinder_off(pin_cylinder_off);
+DigitalIn interrupt(interrupt_pin);
 Serial pc(USBTX,USBRX);
 RS422 rs422(rs422_tx, rs422_rx);
 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
@@ -51,19 +53,25 @@
 Accel v2;
 Accel v3;
 Accel v4;
-Encoder enc_cylinder(encoder_cylinder_A,encoder_cylinder_B);
+
+//追加点
+//Encoder enc_cylinder(encoder_A,encoder_B);
+QEI wheel(encoder_A, encoder_B, NC, 624);
 
 void setup();
 void output();
 void put_output();
 void cylinder_check();
 void boost();
+void cylinder_origin();
 float yaw, target_yaw;
 
 int main()
 {
     setup();
     while(1) {
+        led = interrupt;
+        pc.printf("Pulses is: %i\r\n",wheel.getPulses());
     }
 }
 
@@ -83,8 +91,17 @@
     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);
+    cylinder_origin();
+
+//追加点
+//    enc_cylinder.setup(1200);
+//    enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
+}
+
+void cylinder_origin(){
+    while(interrupt){
+        rs422.put(5, -0.8, 0.0);
+    }
 }
 
 void boost(){
@@ -104,13 +121,13 @@
 
 void cylinder_check()
 {
-    if(sbdbt.sankaku) {
+    if(sbdbt.up) {
         sylinder_on = 1;
     } else {
         sylinder_on = 0;
     }
 
-    if(sbdbt.shikaku) {
+    if(sbdbt.down) {
         sylinder_off = 1;
     } else {
         sylinder_off = 0;
@@ -131,7 +148,7 @@
 void output()
 {
     put_output();
-    //cylinder_check();
+    cylinder_check();
     boost();
 
     static int counter;