Diff: main.cpp

Revision:

19:edf765724d2d

Parent:

18:992846cd1d5d

Child:

20:3b35311f4576

--- a/main.cpp	Mon Nov 18 05:36:49 2019 +0000
+++ b/main.cpp	Mon Nov 18 11:51:49 2019 +0000
@@ -129,9 +129,12 @@
 int SG_Flag=0;
 int SG_Cnt=0;
 int Cross_Flag=0;
+long int Imaginary_Speed=0;
+
 
 int Row=0;    //行変数
 float course_data[100][3];  //記憶走行用配列
+float curvature=0;
 
 void sensor_analog_read(){
     S1_Data=s1.read();
@@ -199,7 +202,34 @@
         Machine_Status &= 0xFB;//右コースアウト情報のみマスク    
     }     
 }
-
+void coner_curvature(){
+                if((motor_a>(motor_b*1.8)) || (motor_a<(motor_b*0.2))){//左が右より80%以上早いか20%以上遅いとき
+                    Imaginary_Speed=Low_Speed;
+                    Target_Speed_A=Imaginary_Speed;
+                    Target_Speed_B=Imaginary_Speed;
+          
+                }else if((motor_a>(motor_b*1.5)) || (motor_a<(motor_b*0.5))){//左が右より50%以上早いか50%以上遅いとき
+                    Imaginary_Speed=Medium_Speed;
+                    Target_Speed_A=Imaginary_Speed;
+                    Target_Speed_B=Imaginary_Speed;
+                }else if((motor_a>(motor_b*1.2)) || (motor_a<(motor_b*0.8))){//左が右より20%以上早いか20%以上遅いとき⇒直線
+                    Imaginary_Speed=High_Speed;
+                    Target_Speed_A=Imaginary_Speed;
+                    Target_Speed_B=Imaginary_Speed;
+                    }
+                else{
+                    Imaginary_Speed=High_Speed;
+                    Target_Speed_A=Imaginary_Speed;
+                    Target_Speed_B=Imaginary_Speed;
+            }
+            course_data[Row][0]=(float)Distance_memory_A;
+            course_data[Row][1]=(float)Distance_memory_B;
+            course_data[Row][2]=Imaginary_Speed;
+            PC.printf("left:%.2f\t",course_data[Row][0]);        
+            PC.printf("right:%.2f\t",course_data[Row][1]);            
+            PC.printf("speed:%.2f\n\r",course_data[Row][2]);               
+                    
+}
 
 //タイマ割り込み1[ms]周期
 void timer_interrupt(){
@@ -232,8 +262,7 @@
             }else if((SG_Flag==1)&&(SG_Cnt==1)){//ゴールスタートマーカの時⇒2回目
                 Machine_Status|=STOP;           //機体停止状態へ
                 SG_Cnt=0;    
-            }else if(Corner_Flag==1){//コーナマーカの時
-                                
+            }else if(Corner_Flag==1){//コーナマーカの時                                
                 Distance_memory_A=0;
                 Distance_memory_B=0;
                 Enc_Count_A=encoder_a.Get();   //エンコーダパルス数を取得
@@ -241,15 +270,15 @@
                 Distance_memory_A=(Enc_Count_A*PULSE_TO_UM);
                 Distance_memory_B=(Enc_Count_B*PULSE_TO_UM);
                                 
-                course_data[Row][0]=Distance_memory_A;
+                /*course_data[Row][0]=Distance_memory_A;
                 course_data[Row][1]=Distance_memory_B;
-                course_data[Row][2]=Medium_Speed;
+                course_data[Row][2]=Imaginary_Speed;
                 
-                /*PC.printf("左:%.2f",course_data[Row][0]);        
+                PC.printf("左:%.2f",course_data[Row][0]);        
                 PC.printf("右:%.2f",course_data[Row][1]);            
                 PC.printf("速度:%.2f",course_data[Row][2]);
                 */
-            
+                coner_curvature();
                 Row++;        
                                                         
             }
@@ -328,7 +357,8 @@
         Motor_A_Diff[0]=(Target_Speed_A1-Speed_A);
         Motor_B_Diff[0]=(Target_Speed_B1-Speed_B);
     }
-    /*
+
+/*
     /////モータの速度制御
     //過去の速度偏差を退避
     Motor_A_Diff[1]=Motor_A_Diff[0];
@@ -341,6 +371,7 @@
     Motor_B_Diff[0]=(Target_Speed_B1-Speed_B);
     */
     
+
     //P成分演算
     Motor_A_P=Motor_A_Diff[0]*M_KP;
     Motor_B_P=Motor_B_Diff[0]*M_KP;
@@ -359,7 +390,7 @@
     else if(Motor_A_Pwm<-0.95)Motor_A_Pwm=-0.95f;
     if(Motor_B_Pwm>0.95f)Motor_B_Pwm=0.95f;
     else if(Motor_B_Pwm<-0.95f)Motor_B_Pwm=-0.95f;
-        
+   /*     
     //モータへの出力    
     if(!(Machine_Status&STOP)){//マシンが停止状態でなければ
         if(Machine_Status&RUN_COURSE_LOUT){         //左端センサ振り切れた時
@@ -379,9 +410,12 @@
     }else{//停止状態の時はモータへの出力は無効
         motor_a=0;
         motor_b=0;        
-    }                    
+   }*/                    
 }
 
+
+
+
 int main() { 
     timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート 
     lcd.cls();//表示クリア