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@42:b00cdc97f492, 2019-05-17 (annotated)

Committer:
shimizuta
Date:
Fri May 17 02:17:40 2019 +0000
Revision:
42:b00cdc97f492
Parent:
39:7f3cc8f47710 
not stop

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 42:b00cdc97f492 65 fprintf(fp, "time[s], gyro[deg], hcsr04_forward[cm], hcsr04_back[cm], motor_lo[deg], motor_li[deg],motor_lo duty, motor_li duty\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 42:b00cdc97f492 88 fprintf(fp, "%f, %f, %f, %f, %f, %f, %f, %f\r\n",
shimizuta 26:5fb1aa9cb7f0 89 filetimer.read(), degree0,
shimizuta 42:b00cdc97f492 90 hcsr04_val[0],
shimizuta 42:b00cdc97f492 91 hcsr04_val[1],
shimizuta 42:b00cdc97f492 92 motor_lo.getPosi(),
shimizuta 42:b00cdc97f492 93 motor_li.getPosi(),
shimizuta 42:b00cdc97f492 94 motor_lo.GetDuty(),
shimizuta 42:b00cdc97f492 95 motor_li.GetDuty());
shimizuta 26:5fb1aa9cb7f0 96 }
shimizuta 26:5fb1aa9cb7f0 97 }
shimizuta 26:5fb1aa9cb7f0 98 void FileClose()
shimizuta 26:5fb1aa9cb7f0 99 {
shimizuta 26:5fb1aa9cb7f0 100 filetimer.stop();
shimizuta 26:5fb1aa9cb7f0 101 if(fp != NULL)
shimizuta 26:5fb1aa9cb7f0 102 fclose(fp);
yuto17320508 33:2dbbe198adaf 103 };
shimizuta 39:7f3cc8f47710 104 void FileCloseFiltered()
shimizuta 39:7f3cc8f47710 105 {
shimizuta 39:7f3cc8f47710 106 //チャタリング防止
shimizuta 39:7f3cc8f47710 107 int is_ok = 0;
shimizuta 39:7f3cc8f47710 108 wait(0.1);
shimizuta 39:7f3cc8f47710 109 if(RightOrLeft) {
shimizuta 39:7f3cc8f47710 110 //rightなら
shimizuta 39:7f3cc8f47710 111 is_ok = (switch_LR == 0);
shimizuta 39:7f3cc8f47710 112 } else {
shimizuta 39:7f3cc8f47710 113 //left
shimizuta 39:7f3cc8f47710 114 is_ok = (switch_LR == 1);
shimizuta 39:7f3cc8f47710 115 }
shimizuta 39:7f3cc8f47710 116 if(is_ok != 1) {
shimizuta 39:7f3cc8f47710 117 return;
shimizuta 39:7f3cc8f47710 118 }
shimizuta 39:7f3cc8f47710 119 FileClose();
shimizuta 39:7f3cc8f47710 120 }
yuto17320508 33:2dbbe198adaf 121
shimizuta 26:5fb1aa9cb7f0 122
shimizuta 22:0ed9de464f40 123
shimizuta 22:0ed9de464f40 124 void TickerSetUp()
shimizuta 22:0ed9de464f40 125 {
shimizuta 26:5fb1aa9cb7f0 126 // ticker.attach(FileWriteTicker,kTicker_s);
shimizuta 22:0ed9de464f40 127 }
shimizuta 22:0ed9de464f40 128
shimizuta 22:0ed9de464f40 129 void Hcsr04Start()
shimizuta 22:0ed9de464f40 130 {
shimizuta 22:0ed9de464f40 131 GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 22:0ed9de464f40 132 GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 22:0ed9de464f40 133 }
shimizuta 21:14133581387b 134 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
shimizuta 21:14133581387b 135 {
shimizuta 39:7f3cc8f47710 136 wait_ms(60);//60
shimizuta 21:14133581387b 137 sensor.start();
shimizuta 22:0ed9de464f40 138 wait_ms(30);
shimizuta 21:14133581387b 139 float raw_data = sensor.get_dist_cm();
shimizuta 22:0ed9de464f40 140 if(raw_data < 20)//0.1以下なら前回の値を返す
shimizuta 21:14133581387b 141 return kOutVal;
shimizuta 26:5fb1aa9cb7f0 142 /* filter.SetData(raw_data);
shimizuta 26:5fb1aa9cb7f0 143 float dist = filter.GetData();
shimizuta 26:5fb1aa9cb7f0 144 // printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
shimizuta 26:5fb1aa9cb7f0 145 return dist;
shimizuta 26:5fb1aa9cb7f0 146 */
yuto17320508 25:c740e6fd5dab 147 return raw_data;
shimizuta 21:14133581387b 148 }
shimizuta 21:14133581387b 149
shimizuta 22:0ed9de464f40 150 float get_dist_forward()
shimizuta 21:14133581387b 151 {
shimizuta 26:5fb1aa9cb7f0 152 hcsr04_val[0] = GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 26:5fb1aa9cb7f0 153 return hcsr04_val[0];
shimizuta 21:14133581387b 154 }
shimizuta 22:0ed9de464f40 155 float get_dist_back()
shimizuta 21:14133581387b 156 {
shimizuta 26:5fb1aa9cb7f0 157 hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 26:5fb1aa9cb7f0 158 return hcsr04_val[1];
shimizuta 21:14133581387b 159 }
shimizuta 22:0ed9de464f40 160
shimizuta 22:0ed9de464f40 161 void set_gyro()
shimizuta 22:0ed9de464f40 162 {
shimizuta 22:0ed9de464f40 163 device.baud(115200);
shimizuta 22:0ed9de464f40 164 device.format(8,Serial::None,1);
shimizuta 22:0ed9de464f40 165 device.attach(dev_rx, Serial::RxIrq);
shimizuta 22:0ed9de464f40 166 wait(0.05);
shimizuta 22:0ed9de464f40 167 theta0=degree0;
shimizuta 22:0ed9de464f40 168 check_gyro();
shimizuta 22:0ed9de464f40 169 }
shimizuta 22:0ed9de464f40 170 void wait_gerege()
shimizuta 22:0ed9de464f40 171 {
shimizuta 22:0ed9de464f40 172 int i = 0;
shimizuta 22:0ed9de464f40 173 while(i!=100) {
shimizuta 22:0ed9de464f40 174 if(hand.read()==0)i++;
shimizuta 22:0ed9de464f40 175 }
shimizuta 22:0ed9de464f40 176 }
shimizuta 22:0ed9de464f40 177 void can_send(int mode, float duty)
shimizuta 22:0ed9de464f40 178 {
shimizuta 22:0ed9de464f40 179 char data[2]= {0};
shimizuta 22:0ed9de464f40 180 int send=mode * 10 + (int)(duty * 10.0);
shimizuta 22:0ed9de464f40 181 data[0]=send & 0b11111111;
shimizuta 22:0ed9de464f40 182 data[1]=hand_mode;
shimizuta 22:0ed9de464f40 183 if(can1.write(CANMessage(0,data,2)))led4=1;
shimizuta 22:0ed9de464f40 184 else led4=0;
shimizuta 22:0ed9de464f40 185 }
shimizuta 26:5fb1aa9cb7f0 186
shimizuta 36:e1398ff45b12 187 float Hcsr04BackWithEc()
shimizuta 36:e1398ff45b12 188 {
shimizuta 36:e1398ff45b12 189
MazeTaka 34:0a8ae7f92262 190 hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
MazeTaka 34:0a8ae7f92262 191 FileWrite();
shimizuta 39:7f3cc8f47710 192 int is_out = checkUW(hcsr04_val[1], degree0*kDeg2Rad, ec_li.getCount());//ECは内側を選んだ.適当
shimizuta 39:7f3cc8f47710 193 if(is_out == 1) {
shimizuta 36:e1398ff45b12 194 //もう一度とる
shimizuta 36:e1398ff45b12 195 sensor_back.start();
shimizuta 36:e1398ff45b12 196 wait_ms(50);
shimizuta 36:e1398ff45b12 197 hcsr04_val[1] = sensor_back.get_dist_cm();
MazeTaka 34:0a8ae7f92262 198 FileWrite();
MazeTaka 34:0a8ae7f92262 199 }
MazeTaka 34:0a8ae7f92262 200 return hcsr04_val[1];
MazeTaka 34:0a8ae7f92262 201 }
shimizuta 36:e1398ff45b12 202 int checkUW(double uwdist,double theta,int eccount) //uwdist[cm],theta[rad],eccount[]
shimizuta 36:e1398ff45b12 203 {
MazeTaka 34:0a8ae7f92262 204 const double oneloopdist=0.22f;//[m]
MazeTaka 34:0a8ae7f92262 205 const double ecsliprate=0.7f;
MazeTaka 34:0a8ae7f92262 206 const double errrange=0.5;//収束判定[m]
MazeTaka 34:0a8ae7f92262 207 //const double D=10;//機体中心から足まで距離[m]
MazeTaka 34:0a8ae7f92262 208 static double X=0,Y=0; //初期位置を代入
MazeTaka 34:0a8ae7f92262 209 static double pre_theta=M_pi*0.5f,pre_eccount=0;
MazeTaka 34:0a8ae7f92262 210 theta+=M_pi*0.5f;
MazeTaka 34:0a8ae7f92262 211 double d_eccount=eccount-pre_eccount;
MazeTaka 34:0a8ae7f92262 212 double d_theta=theta-pre_theta;
MazeTaka 34:0a8ae7f92262 213 double ecdist=oneloopdist*d_eccount/1000*ecsliprate;
MazeTaka 34:0a8ae7f92262 214 double rho=ecdist/d_theta;
MazeTaka 34:0a8ae7f92262 215 double dx=-rho*(sin(pre_theta)-sin(theta));//[m]
MazeTaka 34:0a8ae7f92262 216 double dy= rho*(cos(pre_theta)-cos(theta));//[m]
MazeTaka 34:0a8ae7f92262 217 X+=dx;//[m]
MazeTaka 34:0a8ae7f92262 218 Y+=dy;//[m]
MazeTaka 34:0a8ae7f92262 219 pre_theta=theta;
MazeTaka 34:0a8ae7f92262 220 pre_eccount=eccount;
MazeTaka 34:0a8ae7f92262 221 if(fabs(Y-uwdist*0.01f)<errrange)return 1;//Y-uwdist*0.01f*sin(theta)
MazeTaka 34:0a8ae7f92262 222 else return 0;
MazeTaka 34:0a8ae7f92262 223 }