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@45:6df20a8f4b21, 2019-05-17 (annotated)

Committer:
shimizuta
Date:
Fri May 17 05:43:51 2019 +0000
Revision:
45:6df20a8f4b21
Parent:
36:e1398ff45b12
Child:
47:b77796718a9b
not unsync

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