![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
玉田機体の調整
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Diff: main.cpp
- Revision:
- 15:cfe79ebfcb25
- Parent:
- 14:7ed78f52f40e
- Child:
- 16:017874772ea7
diff -r 7ed78f52f40e -r cfe79ebfcb25 main.cpp --- a/main.cpp Mon Nov 11 06:25:06 2019 +0000 +++ b/main.cpp Fri Nov 15 01:32:08 2019 +0000 @@ -82,12 +82,13 @@ long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納 long int Enc_A_Rotate=0,Enc_B_Rotate=0; -/*long int memory_A=0; +long int memory_A=0; long int memory_B=0; -char memoryA_Str[5]; -char memoryB_Str[5];*/ +char MemoryA_Str[5]; +char MemoryB_Str[5]; +long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] +long int Distance_memory_A=0, Distance_memory_B=0; -long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm] long int Marker_Run_Distance=0; long int Speed_A=0, Speed_B=0; //現在速度 long int Machine_Speed=DEFAULT_SPEED; @@ -237,8 +238,12 @@ Enc_Count_B=-encoder_b.Get(); Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納 Distance_B=(Enc_Count_B*PULSE_TO_UM); - Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s] - Speed_B=(Distance_B*1000)/INTERRUPT_TIME; + + Distance_memory_A=(Enc_Count_A*PULSE_TO_UM); + Distance_memory_B=(Enc_Count_B*PULSE_TO_UM); + + Speed_A=(Distance_A*1000);//走行速度演算[mm/s] + Speed_B=(Distance_B*1000); if(Machine_Status&STOP){//機体停止状態の時 Enc_A_Rotate+=Enc_Count_A;//閾値用に左エンコーダ値の蓄積 @@ -257,8 +262,11 @@ encoder_a.Set(0); encoder_b.Set(0); + memory_A=Distance_memory_A; + memory_B=Distance_memory_B; + /////各モータの目標速度の設定 - if(Sw==0){ + /*if(Sw==0){ Motor_A_Diff[1]=Motor_A_Diff[0]; Motor_B_Diff[1]=Motor_B_Diff[0]; @@ -293,7 +301,7 @@ Motor_B_Diff[0]=(Target_Speed_B1-Speed_B); */ - //P成分演算 + /*//P成分演算 Motor_A_P=Motor_A_Diff[0]*M_KP; Motor_B_P=Motor_B_Diff[0]*M_KP; //D成分演算 @@ -306,7 +314,7 @@ Motor_A_Pwm=Motor_A_PD+Sensor_PD; Motor_B_Pwm=Motor_B_PD-Sensor_PD; - //モータ制御量の上限下限設定 + /* //モータ制御量の上限下限設定 if(Motor_A_Pwm>0.95f)Motor_A_Pwm=0.95f; else if(Motor_A_Pwm<-0.95)Motor_A_Pwm=-0.95f; if(Motor_B_Pwm>0.95f)Motor_B_Pwm=0.95f; @@ -331,27 +339,36 @@ }else{//停止状態の時はモータへの出力は無効 motor_a=0; motor_b=0; - } + }*/ } - - int main() { timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート lcd.cls();//表示クリア lcd.locate(0,0); lcd.print("STOP"); lcd.locate(0,1); - sprintf(Speed_Str,"%04d",Machine_Speed); - lcd.print(Speed_Str); + lcd.print(" "); + + //sprintf(Speed_Str,"%04d",Machine_Speed); + //lcd.print(Speed_Str); + while(1){ Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避 Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得 if((!(Old_Machine_Status&STOP))&&(Machine_Status&STOP)){//走行終了時 + sprintf(MemoryA_Str,"%d",memory_A); + sprintf(MemoryB_Str,"%d",memory_B); lcd.locate(0,0); lcd.print(" "); lcd.locate(0,0); - lcd.print("STOP"); + //lcd.print("STOP"); + + lcd.locate(0,0); + lcd.print(MemoryA_Str); + lcd.locate(0,1); + lcd.print(MemoryB_Str); + Sw++; } @@ -359,27 +376,26 @@ if(Machine_Status&STOP){//機体停止状態の時 Gray=DEFAULT_GRAY+((float)Enc_A_Rotate/16000);//センサ閾値調整 - sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換 - sprintf(Coner_str,"%d",Coner_c); - lcd.locate(0,0); - lcd.print(" "); - lcd.locate(0,0); - lcd.print(Gray_Str); - lcd.print(" "); - lcd.print(Coner_str); + //sprintf(Gray_Str,"%3.2f",Gray);//速度情報文字列変換 + //sprintf(Coner_str,"%d",Coner_c); + + + //lcd.print(Gray_Str); + //lcd.print(" "); + //lcd.print(Coner_str); if(Sw==0){ Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整 sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換 + }else { Machine_Speed1=DEFAULT_SPEED1+(Enc_B_Rotate/16); sprintf(Speed_Str,"%04d",Machine_Speed1);//速度情報文字列変換 } - lcd.locate(0,1); + /*lcd.locate(0,1); lcd.print(" "); lcd.locate(0,1); - lcd.print(Speed_Str); - + lcd.print(Speed_Str);*/ }