佐賀大deラボ機
Dependencies: mbed MAIDROBO BNO055_fusion Ping
main.cpp@0:6c2e6a485519, 2018-10-18 (annotated)
- Committer:
- tajiri1999
- Date:
- Thu Oct 18 09:59:24 2018 +0000
- Revision:
- 0:6c2e6a485519
- Child:
- 1:0d7cc59d485e
????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tajiri1999 | 0:6c2e6a485519 | 1 | #include "mbed.h" |
tajiri1999 | 0:6c2e6a485519 | 2 | #include "BNO055.h" |
tajiri1999 | 0:6c2e6a485519 | 3 | #include "Ping.h" |
tajiri1999 | 0:6c2e6a485519 | 4 | #include "Motor.h" |
tajiri1999 | 0:6c2e6a485519 | 5 | |
tajiri1999 | 0:6c2e6a485519 | 6 | Serial pico(D1,D0); |
tajiri1999 | 0:6c2e6a485519 | 7 | I2C i2c(D14,D15); |
tajiri1999 | 0:6c2e6a485519 | 8 | Ping uss(A1); |
tajiri1999 | 0:6c2e6a485519 | 9 | |
tajiri1999 | 0:6c2e6a485519 | 10 | //タイマー繰り返し割り込み |
tajiri1999 | 0:6c2e6a485519 | 11 | Ticker uss_time; |
tajiri1999 | 0:6c2e6a485519 | 12 | |
tajiri1999 | 0:6c2e6a485519 | 13 | //ボール追従アルゴリズム |
tajiri1999 | 0:6c2e6a485519 | 14 | float r = 0.2; //半径の調整(0~0.7) |
tajiri1999 | 0:6c2e6a485519 | 15 | const float Pi = 3.14159265; |
tajiri1999 | 0:6c2e6a485519 | 16 | |
tajiri1999 | 0:6c2e6a485519 | 17 | //PID関連 |
tajiri1999 | 0:6c2e6a485519 | 18 | Timer TimePID; |
tajiri1999 | 0:6c2e6a485519 | 19 | double dtdeg, preTimedeg, Pdeg, Ideg, Ddeg, prePdeg,Bnodeg, A ,P;//方位PID用変数 |
tajiri1999 | 0:6c2e6a485519 | 20 | double dtpow, preTimepow, Ppow, Ipow, Dpow, prePpow, B;//パワー用PID |
tajiri1999 | 0:6c2e6a485519 | 21 | const float kpdeg = 0.01;//方位センサーPID定数入力 0.005以上はあげない |
tajiri1999 | 0:6c2e6a485519 | 22 | const float kideg = 0.01; |
tajiri1999 | 0:6c2e6a485519 | 23 | const float kddeg = 0.005; |
tajiri1999 | 0:6c2e6a485519 | 24 | const float kppow = 1.5;//パワーPID定数 |
tajiri1999 | 0:6c2e6a485519 | 25 | const float kipow = 0.5; |
tajiri1999 | 0:6c2e6a485519 | 26 | const float kdpow = 0.1; |
tajiri1999 | 0:6c2e6a485519 | 27 | |
tajiri1999 | 0:6c2e6a485519 | 28 | //モーター |
tajiri1999 | 0:6c2e6a485519 | 29 | Motor motor(PB_4,PB_5,PB_3,PA_10,PC_7,PA_9,PA_8,PB_10); |
tajiri1999 | 0:6c2e6a485519 | 30 | |
tajiri1999 | 0:6c2e6a485519 | 31 | //赤外線センサー関連 |
tajiri1999 | 0:6c2e6a485519 | 32 | AnalogIn balldegree(A3);//0のときボール無し10~100のときボール有 |
tajiri1999 | 0:6c2e6a485519 | 33 | float ballbase,ball,nowball,Godeg;//ボール角度用変数,進行方向 |
tajiri1999 | 0:6c2e6a485519 | 34 | AnalogIn balldistance(A2);//0~1で距離を表示ボール無いとき |
tajiri1999 | 0:6c2e6a485519 | 35 | DigitalIn holdsensor(PC_3);//ホールドセンサ |
tajiri1999 | 0:6c2e6a485519 | 36 | |
tajiri1999 | 0:6c2e6a485519 | 37 | |
tajiri1999 | 0:6c2e6a485519 | 38 | |
tajiri1999 | 0:6c2e6a485519 | 39 | |
tajiri1999 | 0:6c2e6a485519 | 40 | //スイッチ類 |
tajiri1999 | 0:6c2e6a485519 | 41 | DigitalIn sw_right(PD_2);//プログラム開始スイッチ//タクト |
tajiri1999 | 0:6c2e6a485519 | 42 | DigitalIn sw_left(PC_11);//ジャイロリセット//タクト |
tajiri1999 | 0:6c2e6a485519 | 43 | DigitalIn swdebug(PC_10);//デバッグモードスイッチhighでon |
tajiri1999 | 0:6c2e6a485519 | 44 | DigitalIn swkick(PC_12);//キーパーモードフォワードモード |
tajiri1999 | 0:6c2e6a485519 | 45 | |
tajiri1999 | 0:6c2e6a485519 | 46 | |
tajiri1999 | 0:6c2e6a485519 | 47 | |
tajiri1999 | 0:6c2e6a485519 | 48 | //プロトタイプ宣言 |
tajiri1999 | 0:6c2e6a485519 | 49 | //****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 50 | //関数名:balldeg(赤外線ボールの角度,赤外線ボールの距離)// |
tajiri1999 | 0:6c2e6a485519 | 51 | //返り値:進行方向 |
tajiri1999 | 0:6c2e6a485519 | 52 | //****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 53 | float balldeg(float ADeg, float ADis); |
tajiri1999 | 0:6c2e6a485519 | 54 | |
tajiri1999 | 0:6c2e6a485519 | 55 | |
tajiri1999 | 0:6c2e6a485519 | 56 | |
tajiri1999 | 0:6c2e6a485519 | 57 | |
tajiri1999 | 0:6c2e6a485519 | 58 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 59 | //関数名:PIDpower(赤外線ボールの角度) |
tajiri1999 | 0:6c2e6a485519 | 60 | //返り値:パワーの操作量を返す |
tajiri1999 | 0:6c2e6a485519 | 61 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 62 | float PIDpower(float BDeg);//パワーの操作量を返す(引数は上と同様) |
tajiri1999 | 0:6c2e6a485519 | 63 | |
tajiri1999 | 0:6c2e6a485519 | 64 | |
tajiri1999 | 0:6c2e6a485519 | 65 | |
tajiri1999 | 0:6c2e6a485519 | 66 | |
tajiri1999 | 0:6c2e6a485519 | 67 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 68 | //関数名:PIDdegree(ジャイロの基準値)////////////////// |
tajiri1999 | 0:6c2e6a485519 | 69 | //返り値:方位の修正角度 |
tajiri1999 | 0:6c2e6a485519 | 70 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 71 | float PIDdegree(int aimdegree);//方位修正操作量を返す |
tajiri1999 | 0:6c2e6a485519 | 72 | |
tajiri1999 | 0:6c2e6a485519 | 73 | |
tajiri1999 | 0:6c2e6a485519 | 74 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 75 | //関数名:talk(プリセット番号を入力) |
tajiri1999 | 0:6c2e6a485519 | 76 | //返り値:無し |
tajiri1999 | 0:6c2e6a485519 | 77 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 78 | void talk(int pat); |
tajiri1999 | 0:6c2e6a485519 | 79 | |
tajiri1999 | 0:6c2e6a485519 | 80 | |
tajiri1999 | 0:6c2e6a485519 | 81 | |
tajiri1999 | 0:6c2e6a485519 | 82 | |
tajiri1999 | 0:6c2e6a485519 | 83 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 84 | //関数名:call_cm(距離(整数)0~300) |
tajiri1999 | 0:6c2e6a485519 | 85 | //返り値:無し |
tajiri1999 | 0:6c2e6a485519 | 86 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 87 | void call_cm(int range); |
tajiri1999 | 0:6c2e6a485519 | 88 | |
tajiri1999 | 0:6c2e6a485519 | 89 | |
tajiri1999 | 0:6c2e6a485519 | 90 | |
tajiri1999 | 0:6c2e6a485519 | 91 | //******************************************************// |
tajiri1999 | 0:6c2e6a485519 | 92 | //関数名:uss_check() |
tajiri1999 | 0:6c2e6a485519 | 93 | //返り値無し |
tajiri1999 | 0:6c2e6a485519 | 94 | //概要:タイマー繰り返し割り込みで使う |
tajiri1999 | 0:6c2e6a485519 | 95 | //*******************************************************// |
tajiri1999 | 0:6c2e6a485519 | 96 | int uss_check(); |
tajiri1999 | 0:6c2e6a485519 | 97 | |
tajiri1999 | 0:6c2e6a485519 | 98 | |
tajiri1999 | 0:6c2e6a485519 | 99 | |
tajiri1999 | 0:6c2e6a485519 | 100 | //BNO055 |
tajiri1999 | 0:6c2e6a485519 | 101 | BNO055 imu(i2c,PC_8); |
tajiri1999 | 0:6c2e6a485519 | 102 | BNO055_ID_INF_TypeDef bno055_id_inf; |
tajiri1999 | 0:6c2e6a485519 | 103 | BNO055_EULER_TypeDef euler_angles; |
tajiri1999 | 0:6c2e6a485519 | 104 | |
tajiri1999 | 0:6c2e6a485519 | 105 | |
tajiri1999 | 0:6c2e6a485519 | 106 | |
tajiri1999 | 0:6c2e6a485519 | 107 | //その他 |
tajiri1999 | 0:6c2e6a485519 | 108 | DigitalIn ZigIn(PA_11);//通信 |
tajiri1999 | 0:6c2e6a485519 | 109 | DigitalOut ZigOut(PB_12); |
tajiri1999 | 0:6c2e6a485519 | 110 | |
tajiri1999 | 0:6c2e6a485519 | 111 | |
tajiri1999 | 0:6c2e6a485519 | 112 | |
tajiri1999 | 0:6c2e6a485519 | 113 | int range;//超音波センサの距離グローバル変数 |
tajiri1999 | 0:6c2e6a485519 | 114 | |
tajiri1999 | 0:6c2e6a485519 | 115 | |
tajiri1999 | 0:6c2e6a485519 | 116 | //****************************************************///// |
tajiri1999 | 0:6c2e6a485519 | 117 | //////////////////////プログラム主文///////////////////////// |
tajiri1999 | 0:6c2e6a485519 | 118 | //*****************************************************///// |
tajiri1999 | 0:6c2e6a485519 | 119 | |
tajiri1999 | 0:6c2e6a485519 | 120 | int main() |
tajiri1999 | 0:6c2e6a485519 | 121 | { |
tajiri1999 | 0:6c2e6a485519 | 122 | |
tajiri1999 | 0:6c2e6a485519 | 123 | //***************************************************//// |
tajiri1999 | 0:6c2e6a485519 | 124 | /////////////////////////初期設定///////////////////////// |
tajiri1999 | 0:6c2e6a485519 | 125 | //***************************************************//// |
tajiri1999 | 0:6c2e6a485519 | 126 | |
tajiri1999 | 0:6c2e6a485519 | 127 | //////////モーターpwm周期設定//////////// |
tajiri1999 | 0:6c2e6a485519 | 128 | pico.baud(9600); |
tajiri1999 | 0:6c2e6a485519 | 129 | pico.printf("?"); |
tajiri1999 | 0:6c2e6a485519 | 130 | motor.setPwmPeriod(0.03); |
tajiri1999 | 0:6c2e6a485519 | 131 | |
tajiri1999 | 0:6c2e6a485519 | 132 | /////////////モードチェンジIMU,COMPASS,M4G,NDOF_FMC_OFF,NDOF//////// |
tajiri1999 | 0:6c2e6a485519 | 133 | imu.reset(); |
tajiri1999 | 0:6c2e6a485519 | 134 | imu.change_fusion_mode(MODE_IMU); |
tajiri1999 | 0:6c2e6a485519 | 135 | wait(0.1); |
tajiri1999 | 0:6c2e6a485519 | 136 | imu.get_Euler_Angles(&euler_angles); |
tajiri1999 | 0:6c2e6a485519 | 137 | float basedegree = 0;//HMC6352read()かeuler_angles.h(ただしIMUのときは0) |
tajiri1999 | 0:6c2e6a485519 | 138 | TimePID.start(); |
tajiri1999 | 0:6c2e6a485519 | 139 | TimePID.reset();//ジャイロリセット |
tajiri1999 | 0:6c2e6a485519 | 140 | preTimedeg = TimePID.read(); |
tajiri1999 | 0:6c2e6a485519 | 141 | motor.omni4Wheels(0,0,0); |
tajiri1999 | 0:6c2e6a485519 | 142 | ///////////////////////////////////////////////////////////////// |
tajiri1999 | 0:6c2e6a485519 | 143 | |
tajiri1999 | 0:6c2e6a485519 | 144 | |
tajiri1999 | 0:6c2e6a485519 | 145 | |
tajiri1999 | 0:6c2e6a485519 | 146 | |
tajiri1999 | 0:6c2e6a485519 | 147 | |
tajiri1999 | 0:6c2e6a485519 | 148 | //************************************************************///// |
tajiri1999 | 0:6c2e6a485519 | 149 | ////////////////////プログラムスタート//////////////////////////////// |
tajiri1999 | 0:6c2e6a485519 | 150 | /////////////////////////////////////////////////////////////////// |
tajiri1999 | 0:6c2e6a485519 | 151 | //************************************************************///// |
tajiri1999 | 0:6c2e6a485519 | 152 | int degreeB,range,direction; |
tajiri1999 | 0:6c2e6a485519 | 153 | while(1) { |
tajiri1999 | 0:6c2e6a485519 | 154 | uss.Send(); |
tajiri1999 | 0:6c2e6a485519 | 155 | wait_ms(40); |
tajiri1999 | 0:6c2e6a485519 | 156 | range = uss.Read_cm(); |
tajiri1999 | 0:6c2e6a485519 | 157 | degreeB = (balldegree.read() - 0.1)*400 - 185; |
tajiri1999 | 0:6c2e6a485519 | 158 | if(sw_right == 1) { |
tajiri1999 | 0:6c2e6a485519 | 159 | if(balldegree.read() > 0.05) { |
tajiri1999 | 0:6c2e6a485519 | 160 | if(degreeB <= -20) { |
tajiri1999 | 0:6c2e6a485519 | 161 | motor.omni4Wheels(0,100,30); |
tajiri1999 | 0:6c2e6a485519 | 162 | direction = 1; |
tajiri1999 | 0:6c2e6a485519 | 163 | } |
tajiri1999 | 0:6c2e6a485519 | 164 | else if(degreeB >= 20) { |
tajiri1999 | 0:6c2e6a485519 | 165 | motor.omni4Wheels(0,100,-30); |
tajiri1999 | 0:6c2e6a485519 | 166 | direction = 2; |
tajiri1999 | 0:6c2e6a485519 | 167 | } |
tajiri1999 | 0:6c2e6a485519 | 168 | else { |
tajiri1999 | 0:6c2e6a485519 | 169 | if(range <= 100) { |
tajiri1999 | 0:6c2e6a485519 | 170 | motor.omni4Wheels(0,0,0); |
tajiri1999 | 0:6c2e6a485519 | 171 | talk(13); |
tajiri1999 | 0:6c2e6a485519 | 172 | } |
tajiri1999 | 0:6c2e6a485519 | 173 | else { |
tajiri1999 | 0:6c2e6a485519 | 174 | motor.omni4Wheels(0,100,-1*degreeB/60); |
tajiri1999 | 0:6c2e6a485519 | 175 | } |
tajiri1999 | 0:6c2e6a485519 | 176 | } |
tajiri1999 | 0:6c2e6a485519 | 177 | } |
tajiri1999 | 0:6c2e6a485519 | 178 | else { |
tajiri1999 | 0:6c2e6a485519 | 179 | if(direction == 1){ |
tajiri1999 | 0:6c2e6a485519 | 180 | motor.omni4Wheels(0,0,100); |
tajiri1999 | 0:6c2e6a485519 | 181 | } |
tajiri1999 | 0:6c2e6a485519 | 182 | else if(direction == 2){ |
tajiri1999 | 0:6c2e6a485519 | 183 | motor.omni4Wheels(0,0,-100); |
tajiri1999 | 0:6c2e6a485519 | 184 | } |
tajiri1999 | 0:6c2e6a485519 | 185 | else{ |
tajiri1999 | 0:6c2e6a485519 | 186 | motor.omni4Wheels(0,0,0); |
tajiri1999 | 0:6c2e6a485519 | 187 | } |
tajiri1999 | 0:6c2e6a485519 | 188 | } |
tajiri1999 | 0:6c2e6a485519 | 189 | } |
tajiri1999 | 0:6c2e6a485519 | 190 | else { |
tajiri1999 | 0:6c2e6a485519 | 191 | if(range <= 200) { |
tajiri1999 | 0:6c2e6a485519 | 192 | talk(10); |
tajiri1999 | 0:6c2e6a485519 | 193 | } |
tajiri1999 | 0:6c2e6a485519 | 194 | direction = 0; |
tajiri1999 | 0:6c2e6a485519 | 195 | motor.omni4Wheels(0,0,0); |
tajiri1999 | 0:6c2e6a485519 | 196 | } |
tajiri1999 | 0:6c2e6a485519 | 197 | |
tajiri1999 | 0:6c2e6a485519 | 198 | } |
tajiri1999 | 0:6c2e6a485519 | 199 | |
tajiri1999 | 0:6c2e6a485519 | 200 | |
tajiri1999 | 0:6c2e6a485519 | 201 | |
tajiri1999 | 0:6c2e6a485519 | 202 | } |
tajiri1999 | 0:6c2e6a485519 | 203 | |
tajiri1999 | 0:6c2e6a485519 | 204 | |
tajiri1999 | 0:6c2e6a485519 | 205 | |
tajiri1999 | 0:6c2e6a485519 | 206 | //******************************************************//////// |
tajiri1999 | 0:6c2e6a485519 | 207 | ///////////進行方向の角度を返す(ボールの角度,ボールの距離)//////////// |
tajiri1999 | 0:6c2e6a485519 | 208 | //********************************************************///// |
tajiri1999 | 0:6c2e6a485519 | 209 | float balldeg(float ADeg, float ADis) |
tajiri1999 | 0:6c2e6a485519 | 210 | { |
tajiri1999 | 0:6c2e6a485519 | 211 | |
tajiri1999 | 0:6c2e6a485519 | 212 | ball = ADeg - 185;//範囲は-180~180 |
tajiri1999 | 0:6c2e6a485519 | 213 | |
tajiri1999 | 0:6c2e6a485519 | 214 | |
tajiri1999 | 0:6c2e6a485519 | 215 | nowball = ball; //範囲は-360~360 |
tajiri1999 | 0:6c2e6a485519 | 216 | |
tajiri1999 | 0:6c2e6a485519 | 217 | if(nowball> 180) { //範囲を-180~180に変換 |
tajiri1999 | 0:6c2e6a485519 | 218 | nowball = -1 * (360 - nowball); |
tajiri1999 | 0:6c2e6a485519 | 219 | } else if(nowball< -180) { |
tajiri1999 | 0:6c2e6a485519 | 220 | nowball = 360 - (-1 * nowball); |
tajiri1999 | 0:6c2e6a485519 | 221 | } |
tajiri1999 | 0:6c2e6a485519 | 222 | |
tajiri1999 | 0:6c2e6a485519 | 223 | if((nowball >= -55) && (nowball <= 55)) { //ボールが前にあるとき |
tajiri1999 | 0:6c2e6a485519 | 224 | return(2*ball); |
tajiri1999 | 0:6c2e6a485519 | 225 | } else if((nowball <= -150) && (nowball >= 150)) { |
tajiri1999 | 0:6c2e6a485519 | 226 | if(nowball >= 0) { |
tajiri1999 | 0:6c2e6a485519 | 227 | Godeg = ball + 90; |
tajiri1999 | 0:6c2e6a485519 | 228 | return(Godeg); |
tajiri1999 | 0:6c2e6a485519 | 229 | } else { |
tajiri1999 | 0:6c2e6a485519 | 230 | Godeg = ball - 90; |
tajiri1999 | 0:6c2e6a485519 | 231 | return(Godeg); |
tajiri1999 | 0:6c2e6a485519 | 232 | } |
tajiri1999 | 0:6c2e6a485519 | 233 | } else { |
tajiri1999 | 0:6c2e6a485519 | 234 | if(ADis <= r) { //ボールとの距離が近いとき(半径) |
tajiri1999 | 0:6c2e6a485519 | 235 | if(nowball >= 0) { |
tajiri1999 | 0:6c2e6a485519 | 236 | Godeg = ball + (180 - 180/Pi*asin(ADis / r)); |
tajiri1999 | 0:6c2e6a485519 | 237 | return(Godeg); |
tajiri1999 | 0:6c2e6a485519 | 238 | } else { |
tajiri1999 | 0:6c2e6a485519 | 239 | Godeg = nowball - (180 - 180/Pi*asin(ADis / r)); |
tajiri1999 | 0:6c2e6a485519 | 240 | return(Godeg); |
tajiri1999 | 0:6c2e6a485519 | 241 | } |
tajiri1999 | 0:6c2e6a485519 | 242 | } else { |
tajiri1999 | 0:6c2e6a485519 | 243 | if(nowball >= 0) { |
tajiri1999 | 0:6c2e6a485519 | 244 | Godeg = ball + 180/Pi * asin(r / ADis);//数値は半径 |
tajiri1999 | 0:6c2e6a485519 | 245 | return(Godeg); |
tajiri1999 | 0:6c2e6a485519 | 246 | } else { |
tajiri1999 | 0:6c2e6a485519 | 247 | Godeg = ball - 180/Pi * asin(r / ADis); |
tajiri1999 | 0:6c2e6a485519 | 248 | return(Godeg); |
tajiri1999 | 0:6c2e6a485519 | 249 | } |
tajiri1999 | 0:6c2e6a485519 | 250 | } |
tajiri1999 | 0:6c2e6a485519 | 251 | } |
tajiri1999 | 0:6c2e6a485519 | 252 | } |
tajiri1999 | 0:6c2e6a485519 | 253 | |
tajiri1999 | 0:6c2e6a485519 | 254 | |
tajiri1999 | 0:6c2e6a485519 | 255 | //**********************************************//// |
tajiri1999 | 0:6c2e6a485519 | 256 | ///////////////PIDでパワーレベルを返す///////////////// |
tajiri1999 | 0:6c2e6a485519 | 257 | //***********************************************/// |
tajiri1999 | 0:6c2e6a485519 | 258 | float PIDpower(float BDeg) |
tajiri1999 | 0:6c2e6a485519 | 259 | { |
tajiri1999 | 0:6c2e6a485519 | 260 | |
tajiri1999 | 0:6c2e6a485519 | 261 | dtpow = TimePID.read() - preTimepow;//微小時間 |
tajiri1999 | 0:6c2e6a485519 | 262 | preTimepow = TimePID.read(); |
tajiri1999 | 0:6c2e6a485519 | 263 | |
tajiri1999 | 0:6c2e6a485519 | 264 | Ppow = BDeg - 185; //ボールの角度 |
tajiri1999 | 0:6c2e6a485519 | 265 | Ipow += Ppow * dtpow; //積分 |
tajiri1999 | 0:6c2e6a485519 | 266 | if(Ppow < 0) { |
tajiri1999 | 0:6c2e6a485519 | 267 | Ppow = Ppow*-1; |
tajiri1999 | 0:6c2e6a485519 | 268 | } |
tajiri1999 | 0:6c2e6a485519 | 269 | Dpow = (Ppow - prePpow)/dtpow;//微分 |
tajiri1999 | 0:6c2e6a485519 | 270 | prePpow = Ppow; |
tajiri1999 | 0:6c2e6a485519 | 271 | |
tajiri1999 | 0:6c2e6a485519 | 272 | B = Ppow * kppow + Ipow * kipow + Dpow * kdpow; //操作量 |
tajiri1999 | 0:6c2e6a485519 | 273 | |
tajiri1999 | 0:6c2e6a485519 | 274 | if((Ppow >= -15) && (Ppow <= 15)) { //ボールが前にあるとき |
tajiri1999 | 0:6c2e6a485519 | 275 | Ipow = 0;//積分初期化 |
tajiri1999 | 0:6c2e6a485519 | 276 | return(90);//パワー最大値 |
tajiri1999 | 0:6c2e6a485519 | 277 | } else if((Ppow >= 90)&&(Ppow <=-90)) { |
tajiri1999 | 0:6c2e6a485519 | 278 | return(90); |
tajiri1999 | 0:6c2e6a485519 | 279 | } else { |
tajiri1999 | 0:6c2e6a485519 | 280 | if(B <= 60) { |
tajiri1999 | 0:6c2e6a485519 | 281 | return(60);//パワー最小値 |
tajiri1999 | 0:6c2e6a485519 | 282 | } else if(B >= 90) { |
tajiri1999 | 0:6c2e6a485519 | 283 | return(90);//パワー最大値 |
tajiri1999 | 0:6c2e6a485519 | 284 | } else { |
tajiri1999 | 0:6c2e6a485519 | 285 | return(B); |
tajiri1999 | 0:6c2e6a485519 | 286 | } |
tajiri1999 | 0:6c2e6a485519 | 287 | } |
tajiri1999 | 0:6c2e6a485519 | 288 | } |
tajiri1999 | 0:6c2e6a485519 | 289 | |
tajiri1999 | 0:6c2e6a485519 | 290 | //*****************************************************/// |
tajiri1999 | 0:6c2e6a485519 | 291 | ///////////PID目標値(角度)を代入すると操作量を返す///////////// |
tajiri1999 | 0:6c2e6a485519 | 292 | //*****************************************************/// |
tajiri1999 | 0:6c2e6a485519 | 293 | float PIDdegree(int aimdegree) |
tajiri1999 | 0:6c2e6a485519 | 294 | { |
tajiri1999 | 0:6c2e6a485519 | 295 | dtdeg = TimePID.read() - preTimedeg;//微小時間 |
tajiri1999 | 0:6c2e6a485519 | 296 | preTimedeg = TimePID.read(); |
tajiri1999 | 0:6c2e6a485519 | 297 | |
tajiri1999 | 0:6c2e6a485519 | 298 | imu.get_Euler_Angles(&euler_angles);//bno055角度取得 |
tajiri1999 | 0:6c2e6a485519 | 299 | Bnodeg = euler_angles.h; |
tajiri1999 | 0:6c2e6a485519 | 300 | Pdeg = Bnodeg - aimdegree;//比例bno055 |
tajiri1999 | 0:6c2e6a485519 | 301 | |
tajiri1999 | 0:6c2e6a485519 | 302 | if(Pdeg > 180) { //角度値を-180~180に修正 |
tajiri1999 | 0:6c2e6a485519 | 303 | Pdeg = -1 * (360 - Pdeg); |
tajiri1999 | 0:6c2e6a485519 | 304 | } else if(Pdeg < -180) { |
tajiri1999 | 0:6c2e6a485519 | 305 | Pdeg = 360 - (-1 * Pdeg); |
tajiri1999 | 0:6c2e6a485519 | 306 | } |
tajiri1999 | 0:6c2e6a485519 | 307 | if(Bnodeg > 180) { //角度値を-180~180に修正 |
tajiri1999 | 0:6c2e6a485519 | 308 | Bnodeg = -1 * (360 - Bnodeg); |
tajiri1999 | 0:6c2e6a485519 | 309 | } else if(Pdeg < -180) { |
tajiri1999 | 0:6c2e6a485519 | 310 | Bnodeg = 360 - (-1 * Bnodeg); |
tajiri1999 | 0:6c2e6a485519 | 311 | } |
tajiri1999 | 0:6c2e6a485519 | 312 | Ideg += Pdeg * dtdeg;//積分 |
tajiri1999 | 0:6c2e6a485519 | 313 | Ddeg = (Pdeg - prePdeg)/dtdeg;//微分 |
tajiri1999 | 0:6c2e6a485519 | 314 | prePdeg = Pdeg; |
tajiri1999 | 0:6c2e6a485519 | 315 | if((Pdeg > -2) && (Pdeg < 2)) { |
tajiri1999 | 0:6c2e6a485519 | 316 | Pdeg = 0; |
tajiri1999 | 0:6c2e6a485519 | 317 | Ideg = 0; |
tajiri1999 | 0:6c2e6a485519 | 318 | Ddeg = 0; |
tajiri1999 | 0:6c2e6a485519 | 319 | |
tajiri1999 | 0:6c2e6a485519 | 320 | } |
tajiri1999 | 0:6c2e6a485519 | 321 | |
tajiri1999 | 0:6c2e6a485519 | 322 | A = Pdeg * kpdeg + Ideg * kideg + Ddeg * kddeg; |
tajiri1999 | 0:6c2e6a485519 | 323 | |
tajiri1999 | 0:6c2e6a485519 | 324 | if(A > 1) { |
tajiri1999 | 0:6c2e6a485519 | 325 | A = 1; |
tajiri1999 | 0:6c2e6a485519 | 326 | } else if(A < -1) { |
tajiri1999 | 0:6c2e6a485519 | 327 | A = -1; |
tajiri1999 | 0:6c2e6a485519 | 328 | } |
tajiri1999 | 0:6c2e6a485519 | 329 | return(A); |
tajiri1999 | 0:6c2e6a485519 | 330 | } |
tajiri1999 | 0:6c2e6a485519 | 331 | |
tajiri1999 | 0:6c2e6a485519 | 332 | |
tajiri1999 | 0:6c2e6a485519 | 333 | //************************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 334 | //関数名:talk(プリセット番号を入力) |
tajiri1999 | 0:6c2e6a485519 | 335 | //返り値:無し |
tajiri1999 | 0:6c2e6a485519 | 336 | //************************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 337 | |
tajiri1999 | 0:6c2e6a485519 | 338 | void talk (int pat) |
tajiri1999 | 0:6c2e6a485519 | 339 | { |
tajiri1999 | 0:6c2e6a485519 | 340 | switch (pat) { |
tajiri1999 | 0:6c2e6a485519 | 341 | case 0: |
tajiri1999 | 0:6c2e6a485519 | 342 | |
tajiri1999 | 0:6c2e6a485519 | 343 | pico.printf("hajimema'site\r"); |
tajiri1999 | 0:6c2e6a485519 | 344 | wait(2); |
tajiri1999 | 0:6c2e6a485519 | 345 | break; |
tajiri1999 | 0:6c2e6a485519 | 346 | case 1: |
tajiri1999 | 0:6c2e6a485519 | 347 | |
tajiri1999 | 0:6c2e6a485519 | 348 | pico.printf("watasiwa/meidoro'bo akemityandayo-?\r"); |
tajiri1999 | 0:6c2e6a485519 | 349 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 350 | break; |
tajiri1999 | 0:6c2e6a485519 | 351 | case 2: |
tajiri1999 | 0:6c2e6a485519 | 352 | |
tajiri1999 | 0:6c2e6a485519 | 353 | pico.printf("mataki'te;ne-?\r"); |
tajiri1999 | 0:6c2e6a485519 | 354 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 355 | break; |
tajiri1999 | 0:6c2e6a485519 | 356 | case 3: |
tajiri1999 | 0:6c2e6a485519 | 357 | |
tajiri1999 | 0:6c2e6a485519 | 358 | pico.printf("yo'-koso o-punkya'npasue\r"); |
tajiri1999 | 0:6c2e6a485519 | 359 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 360 | break; |
tajiri1999 | 0:6c2e6a485519 | 361 | case 4: |
tajiri1999 | 0:6c2e6a485519 | 362 | |
tajiri1999 | 0:6c2e6a485519 | 363 | pico.printf("yamete-?\r"); |
tajiri1999 | 0:6c2e6a485519 | 364 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 365 | break; |
tajiri1999 | 0:6c2e6a485519 | 366 | case 5: |
tajiri1999 | 0:6c2e6a485519 | 367 | |
tajiri1999 | 0:6c2e6a485519 | 368 | pico.printf("ame' do'-zo?\r"); |
tajiri1999 | 0:6c2e6a485519 | 369 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 370 | break; |
tajiri1999 | 0:6c2e6a485519 | 371 | case 6: |
tajiri1999 | 0:6c2e6a485519 | 372 | |
tajiri1999 | 0:6c2e6a485519 | 373 | pico.printf("ko'uko'useinomina'san konnitiwa\r"); |
tajiri1999 | 0:6c2e6a485519 | 374 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 375 | break; |
tajiri1999 | 0:6c2e6a485519 | 376 | case 7: |
tajiri1999 | 0:6c2e6a485519 | 377 | |
tajiri1999 | 0:6c2e6a485519 | 378 | pico.printf("wa'tasiwane? ta'jirisenpaigatuku'ttandayo'.\r"); |
tajiri1999 | 0:6c2e6a485519 | 379 | wait(5); |
tajiri1999 | 0:6c2e6a485519 | 380 | break; |
tajiri1999 | 0:6c2e6a485519 | 381 | case 8: |
tajiri1999 | 0:6c2e6a485519 | 382 | |
tajiri1999 | 0:6c2e6a485519 | 383 | pico.printf("ze'hi ;iwatekenritudaigakunikitekudasai omatisi'teimasu.\r"); |
tajiri1999 | 0:6c2e6a485519 | 384 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 385 | break; |
tajiri1999 | 0:6c2e6a485519 | 386 | case 9: |
tajiri1999 | 0:6c2e6a485519 | 387 | |
tajiri1999 | 0:6c2e6a485519 | 388 | pico.printf("ariga'to-.\r"); |
tajiri1999 | 0:6c2e6a485519 | 389 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 390 | break; |
tajiri1999 | 0:6c2e6a485519 | 391 | case 10: |
tajiri1999 | 0:6c2e6a485519 | 392 | |
tajiri1999 | 0:6c2e6a485519 | 393 | pico.printf("ohayo'-go'zaimasu.\r"); |
tajiri1999 | 0:6c2e6a485519 | 394 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 395 | break; |
tajiri1999 | 0:6c2e6a485519 | 396 | case 11: |
tajiri1999 | 0:6c2e6a485519 | 397 | |
tajiri1999 | 0:6c2e6a485519 | 398 | pico.printf("konnitiwa'\r"); |
tajiri1999 | 0:6c2e6a485519 | 399 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 400 | break; |
tajiri1999 | 0:6c2e6a485519 | 401 | case 12: |
tajiri1999 | 0:6c2e6a485519 | 402 | |
tajiri1999 | 0:6c2e6a485519 | 403 | pico.printf("kyo'umoitinitiotukaresa'ma;\r"); |
tajiri1999 | 0:6c2e6a485519 | 404 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 405 | break; |
tajiri1999 | 0:6c2e6a485519 | 406 | |
tajiri1999 | 0:6c2e6a485519 | 407 | case 13: |
tajiri1999 | 0:6c2e6a485519 | 408 | pico.printf("o\mataseshimashita gosyujinsa'ma;\r"); |
tajiri1999 | 0:6c2e6a485519 | 409 | wait(5); |
tajiri1999 | 0:6c2e6a485519 | 410 | break; |
tajiri1999 | 0:6c2e6a485519 | 411 | |
tajiri1999 | 0:6c2e6a485519 | 412 | |
tajiri1999 | 0:6c2e6a485519 | 413 | } |
tajiri1999 | 0:6c2e6a485519 | 414 | } |
tajiri1999 | 0:6c2e6a485519 | 415 | |
tajiri1999 | 0:6c2e6a485519 | 416 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 417 | //関数名:call_cm(距離(整数)0~300) |
tajiri1999 | 0:6c2e6a485519 | 418 | //返り値:無し |
tajiri1999 | 0:6c2e6a485519 | 419 | //*****************************************************************// |
tajiri1999 | 0:6c2e6a485519 | 420 | void call_cm(int range) |
tajiri1999 | 0:6c2e6a485519 | 421 | { |
tajiri1999 | 0:6c2e6a485519 | 422 | if(range>=300) { |
tajiri1999 | 0:6c2e6a485519 | 423 | pico.printf("sanbyaku\r"); |
tajiri1999 | 0:6c2e6a485519 | 424 | } else if(range>=200) { |
tajiri1999 | 0:6c2e6a485519 | 425 | range= range-200 ; |
tajiri1999 | 0:6c2e6a485519 | 426 | pico.printf("nihyaku"); |
tajiri1999 | 0:6c2e6a485519 | 427 | } else if (range>=100) { |
tajiri1999 | 0:6c2e6a485519 | 428 | range= range-100; |
tajiri1999 | 0:6c2e6a485519 | 429 | pico.printf("hyaku"); |
tajiri1999 | 0:6c2e6a485519 | 430 | } |
tajiri1999 | 0:6c2e6a485519 | 431 | |
tajiri1999 | 0:6c2e6a485519 | 432 | if (range>=90) { |
tajiri1999 | 0:6c2e6a485519 | 433 | range= range-90; |
tajiri1999 | 0:6c2e6a485519 | 434 | pico.printf("kyuujyu-"); |
tajiri1999 | 0:6c2e6a485519 | 435 | } else if(range>=80) { |
tajiri1999 | 0:6c2e6a485519 | 436 | range= range-80; |
tajiri1999 | 0:6c2e6a485519 | 437 | pico.printf("hatijyu-"); |
tajiri1999 | 0:6c2e6a485519 | 438 | } else if(range>=70) { |
tajiri1999 | 0:6c2e6a485519 | 439 | range= range-70; |
tajiri1999 | 0:6c2e6a485519 | 440 | pico.printf("nanajyu-"); |
tajiri1999 | 0:6c2e6a485519 | 441 | } else if(range>=60) { |
tajiri1999 | 0:6c2e6a485519 | 442 | range= range-60; |
tajiri1999 | 0:6c2e6a485519 | 443 | pico.printf("rokujyu-"); |
tajiri1999 | 0:6c2e6a485519 | 444 | } else if(range>=50) { |
tajiri1999 | 0:6c2e6a485519 | 445 | range= range-50; |
tajiri1999 | 0:6c2e6a485519 | 446 | pico.printf("gojyu-"); |
tajiri1999 | 0:6c2e6a485519 | 447 | } else if(range>=40) { |
tajiri1999 | 0:6c2e6a485519 | 448 | range= range-40; |
tajiri1999 | 0:6c2e6a485519 | 449 | pico.printf("yonjyu-"); |
tajiri1999 | 0:6c2e6a485519 | 450 | } else if(range>=30) { |
tajiri1999 | 0:6c2e6a485519 | 451 | range= range-30; |
tajiri1999 | 0:6c2e6a485519 | 452 | pico.printf("sanjyu-"); |
tajiri1999 | 0:6c2e6a485519 | 453 | } else if(range>=20) { |
tajiri1999 | 0:6c2e6a485519 | 454 | range= range-20; |
tajiri1999 | 0:6c2e6a485519 | 455 | pico.printf("nijyu-"); |
tajiri1999 | 0:6c2e6a485519 | 456 | } else if(range>=10) { |
tajiri1999 | 0:6c2e6a485519 | 457 | range= range-10; |
tajiri1999 | 0:6c2e6a485519 | 458 | pico.printf("jyu-"); |
tajiri1999 | 0:6c2e6a485519 | 459 | } |
tajiri1999 | 0:6c2e6a485519 | 460 | |
tajiri1999 | 0:6c2e6a485519 | 461 | if(range==9) { |
tajiri1999 | 0:6c2e6a485519 | 462 | pico.printf("kyuu\r"); |
tajiri1999 | 0:6c2e6a485519 | 463 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 464 | } else if(range==8) { |
tajiri1999 | 0:6c2e6a485519 | 465 | pico.printf("hati\r"); |
tajiri1999 | 0:6c2e6a485519 | 466 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 467 | } else if(range==7) { |
tajiri1999 | 0:6c2e6a485519 | 468 | pico.printf("nana\r"); |
tajiri1999 | 0:6c2e6a485519 | 469 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 470 | } else if(range==6) { |
tajiri1999 | 0:6c2e6a485519 | 471 | pico.printf("roku\r"); |
tajiri1999 | 0:6c2e6a485519 | 472 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 473 | } else if(range==5) { |
tajiri1999 | 0:6c2e6a485519 | 474 | pico.printf("go\r"); |
tajiri1999 | 0:6c2e6a485519 | 475 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 476 | } else if(range==4) { |
tajiri1999 | 0:6c2e6a485519 | 477 | pico.printf("yon\r"); |
tajiri1999 | 0:6c2e6a485519 | 478 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 479 | } else if(range==3) { |
tajiri1999 | 0:6c2e6a485519 | 480 | pico.printf("san\r"); |
tajiri1999 | 0:6c2e6a485519 | 481 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 482 | } else if(range==2) { |
tajiri1999 | 0:6c2e6a485519 | 483 | pico.printf("ni\r"); |
tajiri1999 | 0:6c2e6a485519 | 484 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 485 | } else if(range==1) { |
tajiri1999 | 0:6c2e6a485519 | 486 | pico.printf("iti\r"); |
tajiri1999 | 0:6c2e6a485519 | 487 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 488 | } else { |
tajiri1999 | 0:6c2e6a485519 | 489 | pico.printf("\r"); |
tajiri1999 | 0:6c2e6a485519 | 490 | wait(3); |
tajiri1999 | 0:6c2e6a485519 | 491 | } |
tajiri1999 | 0:6c2e6a485519 | 492 | } |
tajiri1999 | 0:6c2e6a485519 | 493 | |
tajiri1999 | 0:6c2e6a485519 | 494 | //******************************************************// |
tajiri1999 | 0:6c2e6a485519 | 495 | //関数名:uss_check() |
tajiri1999 | 0:6c2e6a485519 | 496 | //返り値無し |
tajiri1999 | 0:6c2e6a485519 | 497 | //概要:タイマー繰り返し割り込みで使う |
tajiri1999 | 0:6c2e6a485519 | 498 | //*******************************************************// |
tajiri1999 | 0:6c2e6a485519 | 499 | int uss_check() |
tajiri1999 | 0:6c2e6a485519 | 500 | { |
tajiri1999 | 0:6c2e6a485519 | 501 | int range = uss.Read_cm(); |
tajiri1999 | 0:6c2e6a485519 | 502 | uss.Send(); |
tajiri1999 | 0:6c2e6a485519 | 503 | return range; |
tajiri1999 | 0:6c2e6a485519 | 504 | } |