Diff: main.cpp

Revision:

33:64fd1bd83bac

Parent:

32:f535ace7c529

Child:

34:02d605c68bf3

--- a/main.cpp	Tue Aug 29 03:01:50 2017 +0000
+++ b/main.cpp	Tue Aug 29 06:56:32 2017 +0000
@@ -41,16 +41,18 @@
 #define pin_interrupt_cylinder_min       p23
 #define encoder_A       p25
 #define encoder_B       p26
-#define enc_Kp      0.009
-#define enc_Ki      0.00
-#define enc_Kd      0.00
-#define powerdown   0.6
-#define pin_servo_reload p21 //
-#define pin_interrupt_reload p22 //
-#define pin_interrupt_sholderright_max  p23
-#define pin_interrupt_sholderright_min  p24
-#define pin_interrupt_sholderleft_max   p29
-#define pin_interrupt_sholderleft_min   p30
+#define enc_Kp      0.005
+#define enc_Ki      0.0001
+#define enc_Kd      0.0003
+#define mecanum_power   0.8
+#define sword_power     0.8
+#define sholder_power   0.8
+//#define pin_servo_reload p29 //
+//#define pin_interrupt_reload p30 //
+#define pin_interrupt_sholderright_max  p21
+#define pin_interrupt_sholderright_min  p22
+#define pin_interrupt_sholderleft_max   p7
+#define pin_interrupt_sholderleft_min   p8
 
 DigitalOut led1(LED1);
 //DigitalOut led2(LED2);
@@ -80,8 +82,8 @@
 //追加点
 Encoder enc_cylinder(encoder_A,encoder_B);
 //QEI wheel(encoder_A, encoder_B, NC, 624);
-Servo servo_reload(pin_servo_reload);
-DigitalIn interrupt_reload(pin_interrupt_reload);
+//Servo servo_reload(pin_servo_reload);
+//DigitalIn interrupt_reload(pin_interrupt_reload);
 
 void setup();
 void output();
@@ -145,17 +147,24 @@
     cylinder_cal();
     sword_cal();
     //boost();
+    
+    if(interrupt_cylinder_min==0){
+        enc_cylinder.origin();
+    }
+    if(sbdbt.left){
+        cylinder_origin();
+    }
 
     static int counter;
     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()*powerdown), v3.duty(mecanum.v3()*powerdown));
+            rs422.put(id[counter], v1.duty(mecanum.v1()*mecanum_power), v3.duty(mecanum.v3()*mecanum_power));
             counter++;
             break;
         case 1:
-            rs422.put(id[counter], v2.duty(mecanum.v2()*powerdown), v4.duty(mecanum.v4()*powerdown));
+            rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power), v4.duty(mecanum.v4()*mecanum_power));
             counter ++;
             break;
         case 2:
@@ -163,11 +172,11 @@
             counter ++;
             break;
         case 3:
-            rs422.put(id[counter], shoulder_right_cal(),shoulder_right_cal());
+            rs422.put(id[counter], shoulder_right_cal()*sholder_power,shoulder_left_cal()*sholder_power);
             counter ++;
             break;
         case 4:
-            rs422.put(id[counter], ((float)sword.getState()*0.8),0.0);
+            rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0);
             counter = 0;
             break;
         default:
@@ -176,20 +185,20 @@
 }
 
 float shoulder_right_cal(){
-    if(interrupt_sholderright_max==1&&sbdbt.batu==1){
+    if(interrupt_sholderright_max==0&&sbdbt.sankaku){
         return 0.0;
     }
-    if(interrupt_sholderright_min==1&&sbdbt.sankaku==1){
-        return 0.0;   
+    if(interrupt_sholderright_min==0&&sbdbt.batu){
+        return 0.0;
     }
     return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
 }
 
 float shoulder_left_cal(){
-    if(interrupt_sholderright_max==1&&sbdbt.up==1){
+    if(interrupt_sholderleft_max==0&&sbdbt.up==1){
         return 0.0;
     }
-    if(interrupt_sholderright_min==1&&sbdbt.down==1){
+    if(interrupt_sholderleft_min==0&&sbdbt.down==1){
         return 0.0;   
     }
     return (sbdbt.up*0.8-sbdbt.down*0.8);   
@@ -226,7 +235,7 @@
     }
     enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //コントローラで数値設定
     
-    pc.printf("terget\t%f\tnow_deg\t%f\tnow_rad\t%f\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.rad(),enc_cylinder.duty());
+    pc.printf("terget\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.pulse(),enc_cylinder.duty());
 
 //リロード機構完成後
     /*