Diff: main.cpp

Revision:

39:6735743ac0f1

Parent:

38:b071512af5ca

Child:

40:2d6888448ab2

--- a/main.cpp	Thu Sep 07 02:21:24 2017 +0000
+++ b/main.cpp	Thu Sep 07 06:58:57 2017 +0000
@@ -39,7 +39,7 @@
 #define pin_interrupt_cylinder_min       p23
 #define encoder_A       p25
 #define encoder_B       p26
-#define enc_Kp      0.005
+#define enc_Kp      0.007
 #define enc_Ki      0.0001
 #define enc_Kd      0.0003
 #define mecanum_power   1.0
@@ -98,9 +98,14 @@
 void servo_origin();
 float shoulder_right_cal();
 float shoulder_left_cal();
-int reload_magazine_flag;
+//output
 float yaw, target_yaw;
+//cylinder_origin
 int cylinder_origin_flag = 0;
+//cylinder
+float cylinder_pwm;
+int cylinder_pos_num = 0;
+float cylinder_pos[3] = {0.0,180.0,360.0};
 
 //riseEventそのうちClassにしたい
 short state;
@@ -119,7 +124,7 @@
     setup();
     while(1) {
         //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg());
-        //pc.printf("rise state : %d\r\n",riseEvent(sbdbt.right));
+        pc.printf("rise state : %d\r\n",cyclic_servo.getState());
     }
 }
 
@@ -135,7 +140,7 @@
     cylinder_origin_first();
     output_timer.attach(&output, output_period);
     yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
-    mecanum.setupdeg(bno055.getYawRad()+180.0);
+    mecanum.setupdeg(bno055.getYawRad());   //基盤が前後逆の場合+180
     v1.setup(acceleration,output_period);
     v2.setup(acceleration,output_period);
     v3.setup(acceleration,output_period);
@@ -179,7 +184,7 @@
 {
     while(interrupt_cylinder_min == 1) {
         led1 = 1;
-        rs422.put(5, -0.2, 0.0);
+        rs422.put(5, -0.5, 0.0);
     }
     led1 = 0;
     enc_cylinder.origin();
@@ -189,18 +194,16 @@
 void cylinder_origin()
 {
     if(cylinder_origin_flag == 1){
-        rs422.put(5, -0.2, 0.0);
+        rs422.put(5, -0.5, 0.0);
     }
     if(interrupt_cylinder_min == 0){
-    cylinder_origin_flag = 0;
-    enc_cylinder.origin();
+        cylinder_origin_flag = 0;
+        enc_cylinder.origin();
+        cylinder_pos_num = 0;
     }
 }
 
 //追記(動作未確認)
-float cylinder_pwm;
-int cylinder_pos_num = 0;
-float cylinder_pos[3] = {0.0,180.0,360.0};
 void cylinder_cal()
 {
     cylinder.cyclic(sbdbt.shikaku);     //cylinder ON/OFF
@@ -234,20 +237,16 @@
 void servo_max(){
     servo.pulsewidth(0.0022);   
 }
-
 void servo_min()
 {
     servo.pulsewidth(0.0010);
 }
-
 void servo_out(){
-    cyclic_servo.cyclic(sbdbt.down);
-    switch (cyclic_servo.getState()){
-        case 0:
-            servo_min();
-        case 1:
-            servo_max();
-        default:
+    cyclic_servo.cyclic(sbdbt.down);    //setServoControl
+    if(cyclic_servo.getState()==0){
+        servo_min();
+    }else if(cyclic_servo.getState()==1){
+        servo_max();
     }
 }
 
@@ -303,11 +302,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), v3.duty(mecanum.v3()*mecanum_power));
+            rs422.put(id[counter], v1.duty(mecanum.v1()*mecanum_power+sbdbt.right_y), v3.duty(mecanum.v3()*mecanum_power+sbdbt.right_y));
             counter++;
             break;
         case 1:
-            rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power), v4.duty(mecanum.v4()*mecanum_power));
+            //.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));
             counter ++;
             break;
         case 2: