ROBOSTEP_5期 / Mbed 2 deprecated George_Master_Param

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed robot

sensors/sensors.cpp@39:7f3cc8f47710, 2019-05-15 (annotated)

Committer:
shimizuta
Date:
Wed May 15 12:53:14 2019 +0000
Revision:
39:7f3cc8f47710
Parent:
36:e1398ff45b12
Child:
42:b00cdc97f492
move oneself

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 21:14133581387b 1 #include "sensors.h"
shimizuta 22:0ed9de464f40 2 #include "microinfinity.h"
shimizuta 22:0ed9de464f40 3 #include "pinnames.h"
shimizuta 26:5fb1aa9cb7f0 4 #include "moves.h"
yuto17320508 33:2dbbe198adaf 5 int RightOrLeft = 0;
yuto17320508 33:2dbbe198adaf 6
shimizuta 26:5fb1aa9cb7f0 7 //lowpass関係
yuto17320508 25:c740e6fd5dab 8 const float kOldWeight = 0;
shimizuta 22:0ed9de464f40 9 const float kOldWeightLight = 0.3; //filterの重み.軽いver
shimizuta 22:0ed9de464f40 10 const float kOutVal = 500; //<0.1が返ってきたときに返す値
shimizuta 21:14133581387b 11 //filter [0]:forward, [1]: back
shimizuta 21:14133581387b 12 LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};
shimizuta 21:14133581387b 13
shimizuta 26:5fb1aa9cb7f0 14 //超音波
shimizuta 22:0ed9de464f40 15 HCSR04 sensor_forward(pin_hcsr04[0][0],pin_hcsr04[0][1]);
shimizuta 22:0ed9de464f40 16 HCSR04 sensor_back(pin_hcsr04[1][0], pin_hcsr04[1][1]);
shimizuta 26:5fb1aa9cb7f0 17 float hcsr04_val[2] = {};
shimizuta 26:5fb1aa9cb7f0 18 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);
MazeTaka 34:0a8ae7f92262 19 const float kDeg2Rad = M_pi/180.0;
MazeTaka 34:0a8ae7f92262 20 int checkUW(double uwdist,double theta,int eccount);
shimizuta 22:0ed9de464f40 21
shimizuta 26:5fb1aa9cb7f0 22 //ec
shimizuta 22:0ed9de464f40 23 const int kResolution=500;
shimizuta 22:0ed9de464f40 24 Ec ec_lo(pin_ec[0][0],pin_ec[0][1],NC,kResolution,0.01);
shimizuta 22:0ed9de464f40 25 Ec ec_li(pin_ec[1][0],pin_ec[1][1],NC,kResolution,0.01);
shimizuta 22:0ed9de464f40 26
shimizuta 26:5fb1aa9cb7f0 27 //スイッチ
shimizuta 22:0ed9de464f40 28 DigitalIn bus_in(pin_bus);
shimizuta 22:0ed9de464f40 29 DigitalIn hand(pin_hand,PullUp);
shimizuta 22:0ed9de464f40 30 DigitalIn switch_lo(pin_switch_lo,PullUp);
shimizuta 22:0ed9de464f40 31 DigitalIn switch_li(pin_switch_li,PullUp);
yuto17320508 33:2dbbe198adaf 32 InterruptIn switch_LR(pin_switch_LR,PullUp);
yuto17320508 33:2dbbe198adaf 33 DigitalIn switch_modes[3]= {
yuto17320508 33:2dbbe198adaf 34 DigitalIn(pin_switch_modes[0],PullUp),
yuto17320508 33:2dbbe198adaf 35 DigitalIn(pin_switch_modes[1],PullUp),
yuto17320508 33:2dbbe198adaf 36 DigitalIn(pin_switch_modes[2],PullUp),
yuto17320508 32:aee87dcaf7ca 37 };
yuto17320508 32:aee87dcaf7ca 38 //InterruptIn mode4(pin_mode4,PullUp);
shimizuta 22:0ed9de464f40 39 DigitalOut led4(LED4);
yuto17320508 35:04699b49c463 40 int hand_mode=G_CLOSE;
shimizuta 21:14133581387b 41
shimizuta 26:5fb1aa9cb7f0 42 //can
shimizuta 22:0ed9de464f40 43 CAN can1(pin_can_rd,pin_can_td);
shimizuta 22:0ed9de464f40 44
shimizuta 26:5fb1aa9cb7f0 45 //ticker
shimizuta 22:0ed9de464f40 46 Ticker ticker;
shimizuta 26:5fb1aa9cb7f0 47 //const float kTicker_s = 0.2;
shimizuta 26:5fb1aa9cb7f0 48 void Hcsr04Start();//超音波をtickerでとる用
shimizuta 26:5fb1aa9cb7f0 49
shimizuta 26:5fb1aa9cb7f0 50
shimizuta 26:5fb1aa9cb7f0 51 //file操作
shimizuta 26:5fb1aa9cb7f0 52 FILE *fp = NULL;
shimizuta 26:5fb1aa9cb7f0 53 LocalFileSystem local("local");
shimizuta 26:5fb1aa9cb7f0 54 const char kFileName[] = "/local/sensors.csv";
shimizuta 26:5fb1aa9cb7f0 55 Timer filetimer;
shimizuta 39:7f3cc8f47710 56 void FileCloseFiltered();
shimizuta 26:5fb1aa9cb7f0 57
yuto17320508 33:2dbbe198adaf 58 int FileOpen() //1:異常終了
shimizuta 26:5fb1aa9cb7f0 59 {
shimizuta 27:d392a95f4799 60 printf("file open\r\n");
shimizuta 26:5fb1aa9cb7f0 61 if ((fp = fopen(kFileName, "w")) == NULL) {
shimizuta 26:5fb1aa9cb7f0 62 printf("error : FileSave()\r\n");
shimizuta 26:5fb1aa9cb7f0 63 return 1;
shimizuta 26:5fb1aa9cb7f0 64 }
shimizuta 26:5fb1aa9cb7f0 65 fprintf(fp, "time[s], gyro[deg], hcsr04_forward[cm], hcsr04_back[cm], motor_lo[deg], motor_li[deg]\r\n");
shimizuta 26:5fb1aa9cb7f0 66 filetimer.reset();
shimizuta 26:5fb1aa9cb7f0 67 filetimer.start();
yuto17320508 33:2dbbe198adaf 68 if(RightOrLeft) {
yuto17320508 33:2dbbe198adaf 69 //rightなら
shimizuta 39:7f3cc8f47710 70 switch_LR.fall(FileCloseFiltered);//left(0)にしたらclose
yuto17320508 33:2dbbe198adaf 71 } else {
yuto17320508 33:2dbbe198adaf 72 //left
shimizuta 39:7f3cc8f47710 73 switch_LR.rise(FileCloseFiltered);//left(0)にしたらclose
yuto17320508 33:2dbbe198adaf 74 }
shimizuta 26:5fb1aa9cb7f0 75 return 0;
shimizuta 26:5fb1aa9cb7f0 76 }
shimizuta 26:5fb1aa9cb7f0 77
shimizuta 26:5fb1aa9cb7f0 78 void FileWrite()
shimizuta 26:5fb1aa9cb7f0 79 {
shimizuta 26:5fb1aa9cb7f0 80 static int is_first = 0;//初回はtimerを0にする
shimizuta 26:5fb1aa9cb7f0 81 if(is_first ==0) {
shimizuta 26:5fb1aa9cb7f0 82 filetimer.reset();
shimizuta 26:5fb1aa9cb7f0 83 ++is_first;
shimizuta 26:5fb1aa9cb7f0 84 }
shimizuta 26:5fb1aa9cb7f0 85 if(fp == NULL) {
shimizuta 26:5fb1aa9cb7f0 86 is_first = 0;
shimizuta 26:5fb1aa9cb7f0 87 } else {
shimizuta 26:5fb1aa9cb7f0 88 fprintf(fp, "%f, %f, %f, %f, %f, %f\r\n",
shimizuta 26:5fb1aa9cb7f0 89 filetimer.read(), degree0,
shimizuta 26:5fb1aa9cb7f0 90 hcsr04_val[0], hcsr04_val[1],
shimizuta 26:5fb1aa9cb7f0 91 motor_lo.getPosi(), motor_li.getPosi());
shimizuta 26:5fb1aa9cb7f0 92 }
shimizuta 26:5fb1aa9cb7f0 93 }
shimizuta 26:5fb1aa9cb7f0 94 void FileClose()
shimizuta 26:5fb1aa9cb7f0 95 {
shimizuta 26:5fb1aa9cb7f0 96 filetimer.stop();
shimizuta 26:5fb1aa9cb7f0 97 if(fp != NULL)
shimizuta 26:5fb1aa9cb7f0 98 fclose(fp);
yuto17320508 33:2dbbe198adaf 99 };
shimizuta 39:7f3cc8f47710 100 void FileCloseFiltered()
shimizuta 39:7f3cc8f47710 101 {
shimizuta 39:7f3cc8f47710 102 //チャタリング防止
shimizuta 39:7f3cc8f47710 103 int is_ok = 0;
shimizuta 39:7f3cc8f47710 104 wait(0.1);
shimizuta 39:7f3cc8f47710 105 if(RightOrLeft) {
shimizuta 39:7f3cc8f47710 106 //rightなら
shimizuta 39:7f3cc8f47710 107 is_ok = (switch_LR == 0);
shimizuta 39:7f3cc8f47710 108 } else {
shimizuta 39:7f3cc8f47710 109 //left
shimizuta 39:7f3cc8f47710 110 is_ok = (switch_LR == 1);
shimizuta 39:7f3cc8f47710 111 }
shimizuta 39:7f3cc8f47710 112 if(is_ok != 1) {
shimizuta 39:7f3cc8f47710 113 return;
shimizuta 39:7f3cc8f47710 114 }
shimizuta 39:7f3cc8f47710 115 FileClose();
shimizuta 39:7f3cc8f47710 116 }
yuto17320508 33:2dbbe198adaf 117
shimizuta 26:5fb1aa9cb7f0 118
shimizuta 22:0ed9de464f40 119
shimizuta 22:0ed9de464f40 120 void TickerSetUp()
shimizuta 22:0ed9de464f40 121 {
shimizuta 26:5fb1aa9cb7f0 122 // ticker.attach(FileWriteTicker,kTicker_s);
shimizuta 22:0ed9de464f40 123 }
shimizuta 22:0ed9de464f40 124
shimizuta 22:0ed9de464f40 125 void Hcsr04Start()
shimizuta 22:0ed9de464f40 126 {
shimizuta 22:0ed9de464f40 127 GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 22:0ed9de464f40 128 GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 22:0ed9de464f40 129 }
shimizuta 21:14133581387b 130 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
shimizuta 21:14133581387b 131 {
shimizuta 39:7f3cc8f47710 132 wait_ms(60);//60
shimizuta 21:14133581387b 133 sensor.start();
shimizuta 22:0ed9de464f40 134 wait_ms(30);
shimizuta 21:14133581387b 135 float raw_data = sensor.get_dist_cm();
shimizuta 22:0ed9de464f40 136 if(raw_data < 20)//0.1以下なら前回の値を返す
shimizuta 21:14133581387b 137 return kOutVal;
shimizuta 26:5fb1aa9cb7f0 138 /* filter.SetData(raw_data);
shimizuta 26:5fb1aa9cb7f0 139 float dist = filter.GetData();
shimizuta 26:5fb1aa9cb7f0 140 // printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
shimizuta 26:5fb1aa9cb7f0 141 return dist;
shimizuta 26:5fb1aa9cb7f0 142 */
yuto17320508 25:c740e6fd5dab 143 return raw_data;
shimizuta 21:14133581387b 144 }
shimizuta 21:14133581387b 145
shimizuta 22:0ed9de464f40 146 float get_dist_forward()
shimizuta 21:14133581387b 147 {
shimizuta 26:5fb1aa9cb7f0 148 hcsr04_val[0] = GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 26:5fb1aa9cb7f0 149 return hcsr04_val[0];
shimizuta 21:14133581387b 150 }
shimizuta 22:0ed9de464f40 151 float get_dist_back()
shimizuta 21:14133581387b 152 {
shimizuta 26:5fb1aa9cb7f0 153 hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 26:5fb1aa9cb7f0 154 return hcsr04_val[1];
shimizuta 21:14133581387b 155 }
shimizuta 22:0ed9de464f40 156
shimizuta 22:0ed9de464f40 157 void set_gyro()
shimizuta 22:0ed9de464f40 158 {
shimizuta 22:0ed9de464f40 159 device.baud(115200);
shimizuta 22:0ed9de464f40 160 device.format(8,Serial::None,1);
shimizuta 22:0ed9de464f40 161 device.attach(dev_rx, Serial::RxIrq);
shimizuta 22:0ed9de464f40 162 wait(0.05);
shimizuta 22:0ed9de464f40 163 theta0=degree0;
shimizuta 22:0ed9de464f40 164 check_gyro();
shimizuta 22:0ed9de464f40 165 }
shimizuta 22:0ed9de464f40 166 void wait_gerege()
shimizuta 22:0ed9de464f40 167 {
shimizuta 22:0ed9de464f40 168 int i = 0;
shimizuta 22:0ed9de464f40 169 while(i!=100) {
shimizuta 22:0ed9de464f40 170 if(hand.read()==0)i++;
shimizuta 22:0ed9de464f40 171 }
shimizuta 22:0ed9de464f40 172 }
shimizuta 22:0ed9de464f40 173 void can_send(int mode, float duty)
shimizuta 22:0ed9de464f40 174 {
shimizuta 22:0ed9de464f40 175 char data[2]= {0};
shimizuta 22:0ed9de464f40 176 int send=mode * 10 + (int)(duty * 10.0);
shimizuta 22:0ed9de464f40 177 data[0]=send & 0b11111111;
shimizuta 22:0ed9de464f40 178 data[1]=hand_mode;
shimizuta 22:0ed9de464f40 179 if(can1.write(CANMessage(0,data,2)))led4=1;
shimizuta 22:0ed9de464f40 180 else led4=0;
shimizuta 22:0ed9de464f40 181 }
shimizuta 26:5fb1aa9cb7f0 182
shimizuta 36:e1398ff45b12 183 float Hcsr04BackWithEc()
shimizuta 36:e1398ff45b12 184 {
shimizuta 36:e1398ff45b12 185
MazeTaka 34:0a8ae7f92262 186 hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
MazeTaka 34:0a8ae7f92262 187 FileWrite();
shimizuta 39:7f3cc8f47710 188 int is_out = checkUW(hcsr04_val[1], degree0*kDeg2Rad, ec_li.getCount());//ECは内側を選んだ.適当
shimizuta 39:7f3cc8f47710 189 if(is_out == 1) {
shimizuta 36:e1398ff45b12 190 //もう一度とる
shimizuta 36:e1398ff45b12 191 sensor_back.start();
shimizuta 36:e1398ff45b12 192 wait_ms(50);
shimizuta 36:e1398ff45b12 193 hcsr04_val[1] = sensor_back.get_dist_cm();
MazeTaka 34:0a8ae7f92262 194 FileWrite();
MazeTaka 34:0a8ae7f92262 195 }
MazeTaka 34:0a8ae7f92262 196 return hcsr04_val[1];
MazeTaka 34:0a8ae7f92262 197 }
shimizuta 36:e1398ff45b12 198 int checkUW(double uwdist,double theta,int eccount) //uwdist[cm],theta[rad],eccount[]
shimizuta 36:e1398ff45b12 199 {
MazeTaka 34:0a8ae7f92262 200 const double oneloopdist=0.22f;//[m]
MazeTaka 34:0a8ae7f92262 201 const double ecsliprate=0.7f;
MazeTaka 34:0a8ae7f92262 202 const double errrange=0.5;//収束判定[m]
MazeTaka 34:0a8ae7f92262 203 //const double D=10;//機体中心から足まで距離[m]
MazeTaka 34:0a8ae7f92262 204 static double X=0,Y=0; //初期位置を代入
MazeTaka 34:0a8ae7f92262 205 static double pre_theta=M_pi*0.5f,pre_eccount=0;
MazeTaka 34:0a8ae7f92262 206 theta+=M_pi*0.5f;
MazeTaka 34:0a8ae7f92262 207 double d_eccount=eccount-pre_eccount;
MazeTaka 34:0a8ae7f92262 208 double d_theta=theta-pre_theta;
MazeTaka 34:0a8ae7f92262 209 double ecdist=oneloopdist*d_eccount/1000*ecsliprate;
MazeTaka 34:0a8ae7f92262 210 double rho=ecdist/d_theta;
MazeTaka 34:0a8ae7f92262 211 double dx=-rho*(sin(pre_theta)-sin(theta));//[m]
MazeTaka 34:0a8ae7f92262 212 double dy= rho*(cos(pre_theta)-cos(theta));//[m]
MazeTaka 34:0a8ae7f92262 213 X+=dx;//[m]
MazeTaka 34:0a8ae7f92262 214 Y+=dy;//[m]
MazeTaka 34:0a8ae7f92262 215 pre_theta=theta;
MazeTaka 34:0a8ae7f92262 216 pre_eccount=eccount;
MazeTaka 34:0a8ae7f92262 217 if(fabs(Y-uwdist*0.01f)<errrange)return 1;//Y-uwdist*0.01f*sin(theta)
MazeTaka 34:0a8ae7f92262 218 else return 0;
MazeTaka 34:0a8ae7f92262 219 }