Diff: main.cpp

Revision:

40:2d6888448ab2

Parent:

39:6735743ac0f1

Child:

42:63aedf71f4d1

--- a/main.cpp	Thu Sep 07 06:58:57 2017 +0000
+++ b/main.cpp	Thu Sep 07 10:12:08 2017 +0000
@@ -33,13 +33,13 @@
 #define yaw_Kp      0.01
 #define yaw_Ki      0.01
 #define yaw_Kd      0.01
-#define acceleration    25
+#define acceleration    20  //25
 #define pin_cylinder_on     p17
 #define pin_cylinder_off    p18
 #define pin_interrupt_cylinder_min       p23
 #define encoder_A       p25
 #define encoder_B       p26
-#define enc_Kp      0.007
+#define enc_Kp      0.0400
 #define enc_Ki      0.0001
 #define enc_Kd      0.0003
 #define mecanum_power   1.0
@@ -55,9 +55,9 @@
 //#define pin_interrupt_reload p30 //
 
 DigitalOut led1(LED1);
-//DigitalOut led2(LED2);
+DigitalOut led2(LED2);
 DigitalOut led3(LED3);
-//DigitalOut led4(LED4);
+DigitalOut led4(LED4);
 DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min);
 Serial pc(USBTX,USBRX);
 RS422 rs422(rs422_tx, rs422_rx);
@@ -123,16 +123,20 @@
 {
     setup();
     while(1) {
+        led1 = interrupt_sholderleft_max;
+        led2 = interrupt_sholderleft_min;
+        led3 = interrupt_sholderright_max;
+        led4 = interrupt_sholderright_min;
         //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg());
-        pc.printf("rise state : %d\r\n",cyclic_servo.getState());
+        //pc.printf("rise state : %d\r\n",cyclic_servo.getState());
     }
 }
 
 void setup()
 {
-    wait(0.5);
+    wait(2.0);
     bno055.begin();
-    wait(0.5);
+    wait(1.0);
     bno055.firstRead();
     pc.baud(pc_baud);
     sbdbt.begin(sbdbt_baud);
@@ -155,10 +159,10 @@
 //Sword
 float shoulder_right_cal()
 {
-    if(interrupt_sholderright_max==0&&sbdbt.sankaku) {
+    if(interrupt_sholderright_max==0&&sbdbt.sankaku==1) {
         return 0.0;
     }
-    if(interrupt_sholderright_min==0&&sbdbt.batu) {
+    if(interrupt_sholderright_min==0&&sbdbt.batu==1) {
         return 0.0;
     }
     return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
@@ -166,13 +170,13 @@
 
 float shoulder_left_cal()
 {
-    if(interrupt_sholderleft_max==0&&sbdbt.up==1) {
+    if(interrupt_sholderleft_max==0&&sbdbt.sankaku==1) {
         return 0.0;
     }
-    if(interrupt_sholderleft_min==0&&sbdbt.down==1) {
+    if(interrupt_sholderleft_min==0&&sbdbt.batu==1) {
         return 0.0;
     }
-    return (sbdbt.sankaku*0.8-sbdbt.batu*0.8);
+    return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
 }
 void sword_cal()
 {
@@ -183,23 +187,20 @@
 void cylinder_origin_first()
 {
     while(interrupt_cylinder_min == 1) {
-        led1 = 1;
-        rs422.put(5, -0.5, 0.0);
+        rs422.put(5, -0.4, 0.0);
     }
-    led1 = 0;
     enc_cylinder.origin();
 }
 
 //cylinder_origin_flagを1で動作する
 void cylinder_origin()
 {
-    if(cylinder_origin_flag == 1){
-        rs422.put(5, -0.5, 0.0);
-    }
-    if(interrupt_cylinder_min == 0){
+    if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1){
         cylinder_origin_flag = 0;
         enc_cylinder.origin();
         cylinder_pos_num = 0;
+    }else if(cylinder_origin_flag == 1){
+        rs422.put(5, -0.5, 0.0);
     }
 }
 
@@ -302,12 +303,12 @@
     int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
     switch (counter) {
         case 0:
-            rs422.put(id[counter], v1.duty(mecanum.v1()*mecanum_power+sbdbt.right_y), v3.duty(mecanum.v3()*mecanum_power+sbdbt.right_y));
+            rs422.put(id[counter], v1.duty((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.5)), v3.duty((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.5)));
             counter++;
             break;
         case 1:
-            //.duty(<cal>*<powerControle>+<boost>)
-            rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power-sbdbt.right_y), v4.duty(mecanum.v4()*mecanum_power-sbdbt.right_y));
+            //.duty(<cal>*<powerControle>+(<boost>*0.5))
+            rs422.put(id[counter], v2.duty((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.5)), v4.duty((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.5)));
             counter ++;
             break;
         case 2: