島根短大 ライントレーサ制作チーム
ライントレースサンプルプログラム。 停止時、右タイヤより速度調整できる機能の追加。 押しボタンスイッチはSTOP,GO機能として割り当て。 停止時、左タイヤの役割は何に割り当てるか検討しておくこと。
Dependencies: mbed AQM0802 CRotaryEncoder TB6612FNG
Revision 11:2cd6f8be124e, committed 2019-08-28
- Comitter:
- yusaku0125
- Date:
- Wed Aug 28 23:14:20 2019 +0000
- Parent:
- 10:e1eb10665472
- Commit message:
- test;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r e1eb10665472 -r 2cd6f8be124e main.cpp
--- a/main.cpp Wed Aug 28 09:44:41 2019 +0000
+++ b/main.cpp Wed Aug 28 23:14:20 2019 +0000
@@ -1,4 +1,4 @@
-////ライントレースサンプルver7
+////ライントレースサンプルver7_1
#include "mbed.h"
#include "CRotaryEncoder.h"
#include "TB6612.h"
@@ -6,9 +6,7 @@
//☆★☆★各種パラメータ調整箇所☆★☆★☆★
-#define FIRST_SPEED 500 //1走目の基本速度[mm/sec]
-#define SECOND_SPEED 750 //2走目の基本速度[mm/sec]
-#define THIRD_SPEED 1000 //3走目の基本速度[mm/sec]
+#define DEFAULT_SPEED 800 //1走目の基本速度[mm/sec]
#define TURN_POWER 0.5 //コースアウト時の旋回力
#define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um]
#define INTERRUPT_TIME 1000 //割りこみ周期[us]
@@ -45,9 +43,6 @@
#define SECOND_RUN 0x02 //機体停止状態
#define TUARD_RUN 0x01 //機体設定モード
-#define FIRST 1
-#define SECOND 2
-#define THIRD 3
//デジタル入力オブジェクト定義
DigitalIn push_sw(D13);
@@ -72,7 +67,6 @@
//使用変数の定義
int Sw_Ptn;
int Old_Sw_Ptn;
-int Run_Mode=FIRST;
double S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data;
double All_Sensor_Data; //ラインセンサ総データ量
double Sensor_Diff[2]={0,0}; //ラインセンサ偏差
@@ -84,7 +78,8 @@
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 Default_Speed=FIRST_SPEED;
+long int Machine_Speed=DEFAULT_SPEED;
+char Speed_Str[5]; //LCD速度表示用文字列
long int Target_Speed_A=0,Target_Speed_B=0; //目標速度
long int Motor_A_Diff[2]={0,0}; //過去の速度偏差と現在の速度偏差を格納
long int Motor_B_Diff[2]={0,0}; //
@@ -233,7 +228,7 @@
if(Machine_Status&STOP){//機体停止状態の時
Enc_B_Rotate+=Enc_Count_B;//モード設定用に右エンコーダ値の蓄積
- if(Enc_B_Rotate<0)Enc_B_Rotate=0;
+ if(Enc_B_Rotate<-6400)Enc_B_Rotate=-6400;
if(Enc_B_Rotate>6400)Enc_B_Rotate=6400;
}
if(Marker_Pass_Flag==1){//マーカ通過中は通過距離情報を蓄積する。
@@ -246,8 +241,8 @@
encoder_b.Set(0);
/////各モータの目標速度の設定
- Target_Speed_A=Default_Speed;
- Target_Speed_B=Default_Speed;
+ Target_Speed_A=Machine_Speed;
+ Target_Speed_B=Machine_Speed;
/////モータの速度制御
//過去の速度偏差を退避
@@ -305,7 +300,8 @@
lcd.locate(0,0);
lcd.print("STOP");
lcd.locate(0,1);
- lcd.print("FIRST");
+ sprintf(Speed_Str,"%04d",Machine_Speed);
+ lcd.print(Speed_Str);
while(1){
Old_Sw_Ptn=Sw_Ptn; //過去のスイッチ入力情報を退避
Sw_Ptn=push_sw; //現在のスイッチ入力情報の取得
@@ -316,38 +312,12 @@
lcd.print("STOP");
}
if(Machine_Status&STOP){//機体停止状態の時
- if((Enc_B_Rotate/1600)<1){//右エンコーダが1回転未満であったら
- if(Run_Mode!=FIRST){
- lcd.cls();//表示クリア
- lcd.locate(0,0);
- lcd.print("STOP");
- lcd.locate(0,1);
- lcd.print("FIRST");
- Run_Mode=FIRST;
- Default_Speed=FIRST_SPEED;
- }
- }else if(((Enc_B_Rotate/1600)>=1)&&((Enc_B_Rotate/1600)<2)){//右エンコーダが1回転以上、2回転未満であったら
- if(Run_Mode!=SECOND){
- lcd.cls();//表示クリア
- lcd.locate(0,0);
- lcd.print("STOP");
- lcd.locate(0,1);
- lcd.print("SECOND");//2走目のモードであることを示す。
- Run_Mode=SECOND;
- Default_Speed=SECOND_SPEED;
- }
- }else if(((Enc_B_Rotate/1600)>=2)&&((Enc_B_Rotate/1600)<3)){//右エンコーダが2回転以上、3回転未満であったら
- if(Run_Mode!=THIRD){
- lcd.cls();//表示クリア
- lcd.locate(0,0);
- lcd.print("STOP");
- lcd.locate(0,1);
- lcd.print("THIRD");//2走目のモードであることを示す。
- Run_Mode=THIRD;
- Default_Speed=THIRD_SPEED;
- }
- }
-
+ Machine_Speed=DEFAULT_SPEED+(Enc_B_Rotate/16);//標準速度調整
+ sprintf(Speed_Str,"%04d",Machine_Speed);//速度情報文字列変換
+ lcd.locate(0,1);
+ lcd.print(" ");
+ lcd.locate(0,1);
+ lcd.print(Speed_Str);
}