Diff: main.cpp

Revision:

5:869dc702b923

Parent:

4:017c55052d44

Child:

6:7afdc6a81566

--- a/main.cpp	Fri Jan 11 07:28:44 2019 +0000
+++ b/main.cpp	Fri Jan 11 09:33:04 2019 +0000
@@ -9,13 +9,14 @@
 
 Ticker timer;
 Timer T;
-QEI Enc(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
 QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING);
 QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING);
+QEI Enc(p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);
 Serial Saber(p13,p14);
 Serial pc(USBTX,USBRX);
 Filter velfilter(INT_TIME);
 
+DigitalIn sw2(p25);
 DigitalIn sw1(p26);//モード切替
 DigitalOut fet1(p22);
 DigitalOut fet2(p21);
@@ -26,6 +27,9 @@
 DigitalIn SENS1(p18);
 DigitalIn SENS2(p17);
 
+DigitalIn G_limit1(p9);
+DigitalIn G_limit2(p10);
+
 int cmd,A;
 int SA1,B_SA1,LIM1,LIM2;
 int S1,S2;
@@ -37,7 +41,7 @@
 
 float encount,b_encount;
 
-int mode = 8;
+int mode = 7;
 int cmd2 = 0;
 int cmd3 = 0;
 
@@ -52,6 +56,13 @@
 
 double filtered_ref_spd;
 
+int G_LIM1=0,G_LIM2=0;
+
+int G_cmd;
+
+int modee=2;
+int button_in_2=0; 
+
 int Button() {
     
     int button_in = sw1.read();
@@ -68,6 +79,23 @@
     
     return sw_state;
 }
+int Button2() {//スイッチのノイズをとる関数
+    
+    button_in_2 = sw2.read();
+    
+    static int pre_button_2 = 0;
+    static int sw_state_2 = 3;
+    
+    if(button_in_2 && pre_button_2)sw_state_2 = 0;
+    if(!button_in_2 && !pre_button_2)sw_state_2 = 3;
+    if(button_in_2 && !pre_button_2)sw_state_2 = 1;
+    if(!button_in_2 && pre_button_2)sw_state_2 = 2;
+    
+    pre_button_2 = button_in_2;
+    
+    return sw_state_2;
+}
+
 
 void timer_warikomi()
 {
@@ -147,55 +175,91 @@
               goal_D=125;
                  if(sw_point==2)mode=3;
                  if(angle>=120)cmd=0;
+                 if(S1==0&&S2==0){
+                fet1=1;
+                A=1;
+                }
                  break;
                
-          case 3:
+          /*case 3:
                 if(sw_point==2)mode=4;
                 if(S1==0&&S2==0){
                 fet1=1;
                 A=1;
                 }
-                break;
+                break;*/
                 
-          case 4:
+          case 3:
                 goal_D=0;
                     if(sw_point==2){
                     cmd=0;
                     b_encount=Enc.getPulses();
-                    mode=5;
+                    mode=4;
                 }
                 break;
-          case 5:
+          case 4:
                 fet1=0;
-                A=0;
-                if(sw_point==2)mode=6;
+                if(sw_point==2)mode=5;
                 break;
            
-          case(6):
+          case 5:
                ref_spd = 26.0;
+               if (sw_point == 2) mode = 6;
+               break;
+        
+          case 6:
+               fet2 = 0;
                if (sw_point == 2) mode = 7;
                break;
         
-          case(7):
-               fet2 = 0;
-               if (sw_point == 2) mode = 8;
-               break;
-        
-          case(8):
+          case 7:
                ref_spd = 0.0;
                fet2 = 1;
                if (sw_point == 2) mode = 0;
-               if (spd3<=0.5) cmd = 0;
+               if (spd3<=5.0) {
+                   cmd2 = 0;
+                   cmd3 = 0;
+                   }
                break;
           }
           
-          if(filtered_ref_spd>=25.5&&mode==6){
-          filtered_ref_spd=26;
-    }else if(filtered_ref_spd>=25.5&&mode==7){
+          G_LIM1=!G_limit1.read();//pullupなので逆
+    G_LIM2=!G_limit2.read();//pullupなので逆
+    
+    int sw_point2 = Button2();//スイッチの関数からリターン
+        
+    if(sw_point2 != 0) switch(modee){
+        case(0):  
+        G_cmd=120;
+        if(G_LIM1==1){
+            G_cmd=0;
+                }
+        if (sw_point2 == 2) modee = 1;
+
+        break;
+        
+        case(1):
+        G_cmd=-120;
+        if (sw_point2 == 2) modee = 2;
+        if(G_LIM2==1){
+            G_cmd=0;
+            }
+        break;
+        
+        case(2)://モータ停止と押し上げ機構の降下
+        G_cmd=0;
+        if (sw_point2 == 2) modee = 0;
+        break;
+        }
+    
+          
+          if(filtered_ref_spd>=25.5&&mode==5){
           filtered_ref_spd=26;
-    }else if(filtered_ref_spd<=5.0&&mode==8){
+    }else if(filtered_ref_spd>=25.5&&mode==6){
+          filtered_ref_spd=26;
+    }else if(filtered_ref_spd<=5.0&&mode==7){
           filtered_ref_spd=0;
-    }else if(mode==6||mode==7||mode==8){
+    }else if(mode==5||mode==6||mode==7){
           filtered_ref_spd = velfilter.SecondOrderLag((double)ref_spd);
     }
     
@@ -241,6 +305,18 @@
         Saber.putc(abs(cmd3));
         Saber.putc((SB_ADRS + 1 + abs(cmd3)) & 0b01111111);
         }
+    if (G_cmd > 0) {
+        Saber.putc(SABER_ADDR);
+        Saber.putc(4);
+        Saber.putc(G_cmd);
+        Saber.putc((SABER_ADDR + 4 + G_cmd) & 0b01111111);
+        }
+    else {
+        Saber.putc(SABER_ADDR);
+        Saber.putc(5);
+        Saber.putc(abs(G_cmd));
+        Saber.putc((SABER_ADDR + 5 + abs(G_cmd)) & 0b01111111);
+        }
         
     pre_spd2 = rot_sp2;
     pre_err2 = spd_err2;
@@ -261,12 +337,15 @@
     fet1=0;
     fet2=1;
     
+    G_limit1.mode( PullUp );    //  内蔵プルアップを使う
+    G_limit2.mode( PullUp ); 
+    
     velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
     
     timer.attach(timer_warikomi,INT_TIME);
     
     while(1) {
         //pc.printf("%d\n",mode);
-        pc.printf("spd2 %f\t spd3 %f\n",spd2,spd3);
+        pc.printf("spd2%f\t spd3 %f\n",spd2,spd3);
     }
 }