ハヤト タジリ
スネーク
Diff: main.cpp
- Revision:
- 1:e49ca9ffcccb
- Parent:
- 0:6c2e6a485519
- Child:
- 2:e1fedc61e5e3
--- a/main.cpp Thu Oct 18 09:59:24 2018 +0000
+++ b/main.cpp Mon Mar 23 11:30:19 2020 +0000
@@ -1,504 +1,216 @@
#include "mbed.h"
-#include "BNO055.h"
-#include "Ping.h"
#include "Motor.h"
-Serial pico(D1,D0);
-I2C i2c(D14,D15);
-Ping uss(A1);
-
-//タイマー繰り返し割り込み
-Ticker uss_time;
-
-//ボール追従アルゴリズム
-float r = 0.2; //半径の調整(0~0.7)
-const float Pi = 3.14159265;
-
-//PID関連
-Timer TimePID;
-double dtdeg, preTimedeg, Pdeg, Ideg, Ddeg, prePdeg,Bnodeg, A ,P;//方位PID用変数
-double dtpow, preTimepow, Ppow, Ipow, Dpow, prePpow, B;//パワー用PID
-const float kpdeg = 0.01;//方位センサーPID定数入力 0.005以上はあげない
-const float kideg = 0.01;
-const float kddeg = 0.005;
-const float kppow = 1.5;//パワーPID定数
-const float kipow = 0.5;
-const float kdpow = 0.1;
-
-//モーター
-Motor motor(PB_4,PB_5,PB_3,PA_10,PC_7,PA_9,PA_8,PB_10);
+//constant
+#define PI 3.1415926
-//赤外線センサー関連
-AnalogIn balldegree(A3);//0のときボール無し10~100のときボール有
-float ballbase,ball,nowball,Godeg;//ボール角度用変数,進行方向
-AnalogIn balldistance(A2);//0~1で距離を表示ボール無いとき
-DigitalIn holdsensor(PC_3);//ホールドセンサ
-
-
-
-
-//スイッチ類
-DigitalIn sw_right(PD_2);//プログラム開始スイッチ//タクト
-DigitalIn sw_left(PC_11);//ジャイロリセット//タクト
-DigitalIn swdebug(PC_10);//デバッグモードスイッチhighでon
-DigitalIn swkick(PC_12);//キーパーモードフォワードモード
-
-
-
-//プロトタイプ宣言
-//****************************************************************//
-//関数名:balldeg(赤外線ボールの角度,赤外線ボールの距離)//
-//返り値:進行方向
-//****************************************************************//
-float balldeg(float ADeg, float ADis);
-
-
-
+//Skps
+Serial ps2(A0,A1,9600);//Tx,Rx(D8,D2)
-//*****************************************************************//
-//関数名:PIDpower(赤外線ボールの角度)
-//返り値:パワーの操作量を返す
-//*****************************************************************//
-float PIDpower(float BDeg);//パワーの操作量を返す(引数は上と同様)
-
-
-
-
-//*****************************************************************//
-//関数名:PIDdegree(ジャイロの基準値)//////////////////
-//返り値:方位の修正角度
-//*****************************************************************//
-float PIDdegree(int aimdegree);//方位修正操作量を返す
-
-
-//*****************************************************************//
-//関数名:talk(プリセット番号を入力)
-//返り値:無し
-//*****************************************************************//
-void talk(int pat);
-
-
-
-
-//*****************************************************************//
-//関数名:call_cm(距離(整数)0~300)
-//返り値:無し
-//*****************************************************************//
-void call_cm(int range);
+//SKPS_Button_number
+enum {
+ //return value：digital
+ p_select,
+ p_joyl,
+ p_joyr,
+ p_start,
+ p_up,
+ p_right,
+ p_down,
+ p_left,
+ p_l2,
+ p_r2,
+ p_l1,
+ p_r1,
+ p_triangle,
+ p_circle,
+ p_cross,
+ p_square,
+ //return_value：analog
+ p_joy_lx,
+ p_joy_ly,
+ p_joy_rx,
+ p_joy_ry,
+ p_joy_lu,
+ p_joy_ld,
+ p_joy_ll,
+ p_joy_lr,
+ p_joy_ru,
+ p_joy_rd,
+ p_joy_rl,
+ p_joy_rr,
+ p_con_status,
+ //return_value：none
+ p_motor1,
+ p_motor2
+};
+//TerminalDisplay(TeraTerm)
+Serial pc(SERIAL_TX, SERIAL_RX);
-//******************************************************//
-//関数名:uss_check()
-//返り値無し
-//概要:タイマー繰り返し割り込みで使う
-//*******************************************************//
-int uss_check();
+//voltage
+AnalogIn voltage(PC_4);
+//UltraSonicSensor
+DigitalIn change(NC);//pa6
+Timer timer_volt;
+//Motor
+Motor motor(PA_12,PA_11,PA_5, PB_2,PB_1,PA_9, PB_15,PB_14,PB_13, PA_8,PB_4,PB_10);
-//BNO055
-BNO055 imu(i2c,PC_8);
-BNO055_ID_INF_TypeDef bno055_id_inf;
-BNO055_EULER_TypeDef euler_angles;
+//kicker
+DigitalOut kick(NC);//pa_8
-
+//ToggleSwitch
+DigitalIn sw_start(PD_2); //program start switch
+DigitalIn sw_reset(PC_12); //gyro sensor reset switch
+DigitalIn sw_kick(USER_BUTTON);
-//その他
-DigitalIn ZigIn(PA_11);//通信
-DigitalOut ZigOut(PB_12);
-
-
-
-int range;//超音波センサの距離グローバル変数
-
-
-//****************************************************/////
-//////////////////////プログラム主文/////////////////////////
-//*****************************************************/////
+//declear prototype (function list)
+bool check_voltage(); //電圧をチェックする
+void uart_send(unsigned char data);
+unsigned char uart_rec(void);
+unsigned char skps(unsigned char data);
+unsigned char skps_vibrate(unsigned char motor, unsigned char value);
+float convert_degree(float x,float y);
+float convert_length(float x,float y);
+/*****************************************************************/
+/**********************main function******************************/
+/*****************************************************************/
int main()
{
- //***************************************************////
- /////////////////////////初期設定/////////////////////////
- //***************************************************////
-
- //////////モーターpwm周期設定////////////
- pico.baud(9600);
- pico.printf("?");
- motor.setPwmPeriod(0.03);
+//**************************************************************//
+////////////////////////initialize setting////////////////////////
+//**************************************************************//
+ wait_ms(50);
+ kick = 0; //キックを解除する
- /////////////モードチェンジIMU,COMPASS,M4G,NDOF_FMC_OFF,NDOF////////
- imu.reset();
- imu.change_fusion_mode(MODE_IMU);
- wait(0.1);
- imu.get_Euler_Angles(&euler_angles);
- float basedegree = 0;//HMC6352read()かeuler_angles.h(ただしIMUのときは0)
- TimePID.start();
- TimePID.reset();//ジャイロリセット
- preTimedeg = TimePID.read();
- motor.omni4Wheels(0,0,0);
- /////////////////////////////////////////////////////////////////
-
-
-
-
+ /*motor pwm frequency set*/
+ motor.setPwmPeriod(0.00052);
- //************************************************************/////
- ////////////////////プログラムスタート////////////////////////////////
- ///////////////////////////////////////////////////////////////////
- //************************************************************/////
- int degreeB,range,direction;
- while(1) {
- uss.Send();
- wait_ms(40);
- range = uss.Read_cm();
- degreeB = (balldegree.read() - 0.1)*400 - 185;
- if(sw_right == 1) {
- if(balldegree.read() > 0.05) {
- if(degreeB <= -20) {
- motor.omni4Wheels(0,100,30);
- direction = 1;
- }
- else if(degreeB >= 20) {
- motor.omni4Wheels(0,100,-30);
- direction = 2;
- }
- else {
- if(range <= 100) {
- motor.omni4Wheels(0,0,0);
- talk(13);
- }
- else {
- motor.omni4Wheels(0,100,-1*degreeB/60);
- }
- }
- }
- else {
- if(direction == 1){
- motor.omni4Wheels(0,0,100);
- }
- else if(direction == 2){
- motor.omni4Wheels(0,0,-100);
- }
- else{
- motor.omni4Wheels(0,0,0);
- }
+ while (1) {
+//***************************************************************//
+////////////////Play mode////////////
+//***************************************************************//
+ timer_volt.reset();
+ timer_volt.start();
+ while (sw_start == 0 && check_voltage() == 1) {
+ float deg,power,yaw_angle,lx,ly,rx;
+
+ //convert int to float
+ lx = (float)skps(p_joy_lx);
+ ly = (float)skps(p_joy_ly);
+ rx = (float)skps(p_joy_rx);
+ deg = convert_degree(lx,ly);
+
+ yaw_angle = (rx - 128)/128;
+
+ power = convert_length(lx,ly);
+ if(power <= 10) { //遊びをカット
+ power = 0;
}
- }
- else {
- if(range <= 200) {
- talk(10);
- }
- direction = 0;
- motor.omni4Wheels(0,0,0);
- }
-
- }
-
-
-
-}
-
-
-
-//******************************************************////////
-///////////進行方向の角度を返す(ボールの角度,ボールの距離)////////////
-//********************************************************/////
-float balldeg(float ADeg, float ADis)
-{
-
- ball = ADeg - 185;//範囲は-180~180
-
-
- nowball = ball; //範囲は-360~360
-
- if(nowball> 180) { //範囲を-180~180に変換
- nowball = -1 * (360 - nowball);
- } else if(nowball< -180) {
- nowball = 360 - (-1 * nowball);
- }
-
- if((nowball >= -55) && (nowball <= 55)) { //ボールが前にあるとき
- return(2*ball);
- } else if((nowball <= -150) && (nowball >= 150)) {
- if(nowball >= 0) {
- Godeg = ball + 90;
- return(Godeg);
- } else {
- Godeg = ball - 90;
- return(Godeg);
- }
- } else {
- if(ADis <= r) { //ボールとの距離が近いとき(半径)
- if(nowball >= 0) {
- Godeg = ball + (180 - 180/Pi*asin(ADis / r));
- return(Godeg);
- } else {
- Godeg = nowball - (180 - 180/Pi*asin(ADis / r));
- return(Godeg);
+
+ if(skps(p_l1) == 0 || skps(p_square) == 0){
+ motor.omniWheels(deg,power,yaw_angle*30);
}
- } else {
- if(nowball >= 0) {
- Godeg = ball + 180/Pi * asin(r / ADis);//数値は半径
- return(Godeg);
- } else {
- Godeg = ball - 180/Pi * asin(r / ADis);
- return(Godeg);
+ else{
+ motor.omniWheels(deg,power*0.5,yaw_angle*30);
+ }
+ /*kick*/
+ if(skps(p_circle) == 0 || skps(p_r1)== 0){
+ kick = 1;
+ wait_ms(100);
+ kick = 0;
}
}
+ motor.omniWheels(0, 0, 0);
+ /**********************end main function**************************/
}
}
-//**********************************************////
-///////////////PIDでパワーレベルを返す/////////////////
-//***********************************************///
-float PIDpower(float BDeg)
+//////////////////////////////////////
+/*Battery check voltage*/
+//////////////////////////////////////
+bool check_voltage()
{
-
- dtpow = TimePID.read() - preTimepow;//微小時間
- preTimepow = TimePID.read();
-
- Ppow = BDeg - 185; //ボールの角度
- Ipow += Ppow * dtpow; //積分
- if(Ppow < 0) {
- Ppow = Ppow*-1;
- }
- Dpow = (Ppow - prePpow)/dtpow;//微分
- prePpow = Ppow;
+ static float sum[5] = { 8, 8, 8, 8, 8 };
+ static float Ave = 8;
+ static bool S = 1;
+ if (timer_volt.read() > 0.2) {
+ for (int i = 4; i > 0; i--) {
+ sum[i] = sum[i - 1];
+ }
+ sum[0] = voltage.read() * 9.9;
+ Ave = (sum[0] + sum[1] + sum[2] + sum[3] + sum[4]) / 5;
+ if (Ave < 7.0) { //7.0V以下で自動遮断
+ S = 0;
+ } else {
+ S = 1;
- B = Ppow * kppow + Ipow * kipow + Dpow * kdpow; //操作量
-
- if((Ppow >= -15) && (Ppow <= 15)) { //ボールが前にあるとき
- Ipow = 0;//積分初期化
- return(90);//パワー最大値
- } else if((Ppow >= 90)&&(Ppow <=-90)) {
- return(90);
- } else {
- if(B <= 60) {
- return(60);//パワー最小値
- } else if(B >= 90) {
- return(90);//パワー最大値
- } else {
- return(B);
}
+ timer_volt.reset();
}
+ return S;
}
-//*****************************************************///
-///////////PID目標値(角度)を代入すると操作量を返す/////////////
-//*****************************************************///
-float PIDdegree(int aimdegree)
+void uart_send(unsigned char data)
{
- dtdeg = TimePID.read() - preTimedeg;//微小時間
- preTimedeg = TimePID.read();
-
- imu.get_Euler_Angles(&euler_angles);//bno055角度取得
- Bnodeg = euler_angles.h;
- Pdeg = Bnodeg - aimdegree;//比例bno055
+ ps2.putc(data);
+}
- if(Pdeg > 180) { //角度値を-180~180に修正
- Pdeg = -1 * (360 - Pdeg);
- } else if(Pdeg < -180) {
- Pdeg = 360 - (-1 * Pdeg);
- }
- if(Bnodeg > 180) { //角度値を-180~180に修正
- Bnodeg = -1 * (360 - Bnodeg);
- } else if(Pdeg < -180) {
- Bnodeg = 360 - (-1 * Bnodeg);
- }
- Ideg += Pdeg * dtdeg;//積分
- Ddeg = (Pdeg - prePdeg)/dtdeg;//微分
- prePdeg = Pdeg;
- if((Pdeg > -2) && (Pdeg < 2)) {
- Pdeg = 0;
- Ideg = 0;
- Ddeg = 0;
-
- }
-
- A = Pdeg * kpdeg + Ideg * kideg + Ddeg * kddeg;
-
- if(A > 1) {
- A = 1;
- } else if(A < -1) {
- A = -1;
- }
- return(A);
+unsigned char uart_rec(void)
+{
+ unsigned char temp;
+ while(ps2.readable() == 0);
+ temp = ps2.getc();
+ return temp;
}
-
-//************************************************************************//
-//関数名:talk(プリセット番号を入力)
-//返り値:無し
-//************************************************************************//
-
-void talk (int pat)
+unsigned char skps(unsigned char data)
{
- switch (pat) {
- case 0:
-
- pico.printf("hajimema'site\r");
- wait(2);
- break;
- case 1:
-
- pico.printf("watasiwa/meidoro'bo akemityandayo-?\r");
- wait(3);
- break;
- case 2:
-
- pico.printf("mataki'te;ne-?\r");
- wait(3);
- break;
- case 3:
-
- pico.printf("yo'-koso o-punkya'npasue\r");
- wait(3);
- break;
- case 4:
-
- pico.printf("yamete-?\r");
- wait(3);
- break;
- case 5:
-
- pico.printf("ame' do'-zo?\r");
- wait(3);
- break;
- case 6:
+ uart_send(data);
+ return uart_rec();
+}
- pico.printf("ko'uko'useinomina'san konnitiwa\r");
- wait(3);
- break;
- case 7:
-
- pico.printf("wa'tasiwane? ta'jirisenpaigatuku'ttandayo'.\r");
- wait(5);
- break;
- case 8:
-
- pico.printf("ze'hi ;iwatekenritudaigakunikitekudasai omatisi'teimasu.\r");
- wait(3);
- break;
- case 9:
-
- pico.printf("ariga'to-.\r");
- wait(3);
- break;
- case 10:
-
- pico.printf("ohayo'-go'zaimasu.\r");
- wait(3);
- break;
- case 11:
-
- pico.printf("konnitiwa'\r");
- wait(3);
- break;
- case 12:
-
- pico.printf("kyo'umoitinitiotukaresa'ma;\r");
- wait(3);
- break;
-
- case 13:
- pico.printf("o\mataseshimashita gosyujinsa'ma;\r");
- wait(5);
- break;
-
-
- }
+unsigned char skps_vibrate(unsigned char motor, unsigned char value)
+{
+ uart_send(motor);
+ uart_send(value);
}
-//*****************************************************************//
-//関数名:call_cm(距離(整数)0~300)
-//返り値:無し
-//*****************************************************************//
-void call_cm(int range)
-{
- if(range>=300) {
- pico.printf("sanbyaku\r");
- } else if(range>=200) {
- range= range-200 ;
- pico.printf("nihyaku");
- } else if (range>=100) {
- range= range-100;
- pico.printf("hyaku");
- }
-
- if (range>=90) {
- range= range-90;
- pico.printf("kyuujyu-");
- } else if(range>=80) {
- range= range-80;
- pico.printf("hatijyu-");
- } else if(range>=70) {
- range= range-70;
- pico.printf("nanajyu-");
- } else if(range>=60) {
- range= range-60;
- pico.printf("rokujyu-");
- } else if(range>=50) {
- range= range-50;
- pico.printf("gojyu-");
- } else if(range>=40) {
- range= range-40;
- pico.printf("yonjyu-");
- } else if(range>=30) {
- range= range-30;
- pico.printf("sanjyu-");
- } else if(range>=20) {
- range= range-20;
- pico.printf("nijyu-");
- } else if(range>=10) {
- range= range-10;
- pico.printf("jyu-");
- }
-
- if(range==9) {
- pico.printf("kyuu\r");
- wait(3);
- } else if(range==8) {
- pico.printf("hati\r");
- wait(3);
- } else if(range==7) {
- pico.printf("nana\r");
- wait(3);
- } else if(range==6) {
- pico.printf("roku\r");
- wait(3);
- } else if(range==5) {
- pico.printf("go\r");
- wait(3);
- } else if(range==4) {
- pico.printf("yon\r");
- wait(3);
- } else if(range==3) {
- pico.printf("san\r");
- wait(3);
- } else if(range==2) {
- pico.printf("ni\r");
- wait(3);
- } else if(range==1) {
- pico.printf("iti\r");
- wait(3);
- } else {
- pico.printf("\r");
- wait(3);
- }
+/**************************************************************/
+/*関数名：convert_degree(float x,float y)*/
+/*引数：ジョイスティックのX,Y Axis*/
+/*戻り値：float型 極座標の角度(前方が0度,時計回りに360度)*/
+/*概要:ロボットの進行方向を決めるときに使う*/
+/*************************************************************/
+float convert_degree(float x,float y){
+ float degree;
+ degree = (180/PI)*atan2(y - 128,x - 128) + 360 + 90;//オイラー角度に変換
+
+ //角度を0~360度に変換
+ if(degree >= 360.0){
+ degree = degree - 360;
+ }
+ return degree;
}
-//******************************************************//
-//関数名:uss_check()
-//返り値無し
-//概要:タイマー繰り返し割り込みで使う
-//*******************************************************//
-int uss_check()
-{
- int range = uss.Read_cm();
- uss.Send();
- return range;
+/***************************************************************/
+/*関数名:convert_length(float x,flaot y)*/
+/*引数:ジョイスティックのX,Y*/
+/*戻り値：float型 極座標の原点から(x,y)点までの長さを0~100で返す*/
+/*概要:モーターのパワーを決めるときに,極座標の長さを使う*/
+/***************************************************************/
+float convert_length(float x,float y){
+ float xy_length;
+ xy_length = pow(x - 128,2) + pow(y - 128,2);
+ xy_length = (100 * xy_length)/(128*128);
+ if(xy_length >= 100.0){
+ xy_length = 100.0;
+ }
+ return xy_length;
}
\ No newline at end of file