ハヤト タジリ / Mbed 2 deprecated Robot_for_Saga_univ_de_lab

佐賀大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?

UserRevisionLine numberNew 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 }