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@34:0a8ae7f92262, 2019-05-13 (annotated)

Committer:
MazeTaka
Date:
Mon May 13 11:55:08 2019 +0000
Revision:
34:0a8ae7f92262
Parent:
33:2dbbe198adaf
Child:
35:04699b49c463
checkUW;

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);
shimizuta 22:0ed9de464f40 40 int hand_mode=NORMAL;
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 26:5fb1aa9cb7f0 56
yuto17320508 33:2dbbe198adaf 57 int FileOpen() //1:異常終了
shimizuta 26:5fb1aa9cb7f0 58 {
shimizuta 27:d392a95f4799 59 printf("file open\r\n");
shimizuta 26:5fb1aa9cb7f0 60 if ((fp = fopen(kFileName, "w")) == NULL) {
shimizuta 26:5fb1aa9cb7f0 61 printf("error : FileSave()\r\n");
shimizuta 26:5fb1aa9cb7f0 62 return 1;
shimizuta 26:5fb1aa9cb7f0 63 }
shimizuta 26:5fb1aa9cb7f0 64 fprintf(fp, "time[s], gyro[deg], hcsr04_forward[cm], hcsr04_back[cm], motor_lo[deg], motor_li[deg]\r\n");
shimizuta 26:5fb1aa9cb7f0 65 filetimer.reset();
shimizuta 26:5fb1aa9cb7f0 66 filetimer.start();
yuto17320508 33:2dbbe198adaf 67 if(RightOrLeft) {
yuto17320508 33:2dbbe198adaf 68 //rightなら
yuto17320508 33:2dbbe198adaf 69 switch_LR.fall(FileClose);//left(0)にしたらclose
yuto17320508 33:2dbbe198adaf 70 } else {
yuto17320508 33:2dbbe198adaf 71 //left
yuto17320508 33:2dbbe198adaf 72 switch_LR.rise(FileClose);//left(0)にしたらclose
yuto17320508 33:2dbbe198adaf 73 }
shimizuta 26:5fb1aa9cb7f0 74 return 0;
shimizuta 26:5fb1aa9cb7f0 75 }
shimizuta 26:5fb1aa9cb7f0 76
shimizuta 26:5fb1aa9cb7f0 77 void FileWrite()
shimizuta 26:5fb1aa9cb7f0 78 {
shimizuta 26:5fb1aa9cb7f0 79 static int is_first = 0;//初回はtimerを0にする
shimizuta 26:5fb1aa9cb7f0 80 if(is_first ==0) {
shimizuta 26:5fb1aa9cb7f0 81 filetimer.reset();
shimizuta 26:5fb1aa9cb7f0 82 ++is_first;
shimizuta 26:5fb1aa9cb7f0 83 }
shimizuta 26:5fb1aa9cb7f0 84 if(fp == NULL) {
shimizuta 26:5fb1aa9cb7f0 85 is_first = 0;
shimizuta 26:5fb1aa9cb7f0 86 } else {
shimizuta 26:5fb1aa9cb7f0 87 fprintf(fp, "%f, %f, %f, %f, %f, %f\r\n",
shimizuta 26:5fb1aa9cb7f0 88 filetimer.read(), degree0,
shimizuta 26:5fb1aa9cb7f0 89 hcsr04_val[0], hcsr04_val[1],
shimizuta 26:5fb1aa9cb7f0 90 motor_lo.getPosi(), motor_li.getPosi());
shimizuta 26:5fb1aa9cb7f0 91 }
shimizuta 26:5fb1aa9cb7f0 92 }
shimizuta 26:5fb1aa9cb7f0 93 void FileClose()
shimizuta 26:5fb1aa9cb7f0 94 {
shimizuta 26:5fb1aa9cb7f0 95 filetimer.stop();
shimizuta 26:5fb1aa9cb7f0 96 if(fp != NULL)
shimizuta 26:5fb1aa9cb7f0 97 fclose(fp);
yuto17320508 33:2dbbe198adaf 98 };
yuto17320508 33:2dbbe198adaf 99
shimizuta 26:5fb1aa9cb7f0 100
shimizuta 22:0ed9de464f40 101
shimizuta 22:0ed9de464f40 102 void TickerSetUp()
shimizuta 22:0ed9de464f40 103 {
shimizuta 26:5fb1aa9cb7f0 104 // ticker.attach(FileWriteTicker,kTicker_s);
shimizuta 22:0ed9de464f40 105 }
shimizuta 22:0ed9de464f40 106
shimizuta 22:0ed9de464f40 107 void Hcsr04Start()
shimizuta 22:0ed9de464f40 108 {
shimizuta 22:0ed9de464f40 109 GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 22:0ed9de464f40 110 GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 22:0ed9de464f40 111 }
shimizuta 21:14133581387b 112 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
shimizuta 21:14133581387b 113 {
shimizuta 27:d392a95f4799 114 wait_ms(90);//60
shimizuta 21:14133581387b 115 sensor.start();
shimizuta 22:0ed9de464f40 116 wait_ms(30);
shimizuta 21:14133581387b 117 float raw_data = sensor.get_dist_cm();
shimizuta 22:0ed9de464f40 118 if(raw_data < 20)//0.1以下なら前回の値を返す
shimizuta 21:14133581387b 119 return kOutVal;
shimizuta 26:5fb1aa9cb7f0 120 /* filter.SetData(raw_data);
shimizuta 26:5fb1aa9cb7f0 121 float dist = filter.GetData();
shimizuta 26:5fb1aa9cb7f0 122 // printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
shimizuta 26:5fb1aa9cb7f0 123 return dist;
shimizuta 26:5fb1aa9cb7f0 124 */
yuto17320508 25:c740e6fd5dab 125 return raw_data;
shimizuta 21:14133581387b 126 }
shimizuta 21:14133581387b 127
shimizuta 22:0ed9de464f40 128 float get_dist_forward()
shimizuta 21:14133581387b 129 {
shimizuta 26:5fb1aa9cb7f0 130 hcsr04_val[0] = GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 26:5fb1aa9cb7f0 131 return hcsr04_val[0];
shimizuta 21:14133581387b 132 }
shimizuta 22:0ed9de464f40 133 float get_dist_back()
shimizuta 21:14133581387b 134 {
shimizuta 26:5fb1aa9cb7f0 135 hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 26:5fb1aa9cb7f0 136 return hcsr04_val[1];
shimizuta 21:14133581387b 137 }
shimizuta 22:0ed9de464f40 138
shimizuta 22:0ed9de464f40 139 void set_gyro()
shimizuta 22:0ed9de464f40 140 {
shimizuta 22:0ed9de464f40 141 device.baud(115200);
shimizuta 22:0ed9de464f40 142 device.format(8,Serial::None,1);
shimizuta 22:0ed9de464f40 143 device.attach(dev_rx, Serial::RxIrq);
shimizuta 22:0ed9de464f40 144 wait(0.05);
shimizuta 22:0ed9de464f40 145 theta0=degree0;
shimizuta 22:0ed9de464f40 146 check_gyro();
shimizuta 22:0ed9de464f40 147 }
shimizuta 22:0ed9de464f40 148 void wait_gerege()
shimizuta 22:0ed9de464f40 149 {
shimizuta 22:0ed9de464f40 150 int i = 0;
shimizuta 22:0ed9de464f40 151 while(i!=100) {
shimizuta 22:0ed9de464f40 152 if(hand.read()==0)i++;
shimizuta 22:0ed9de464f40 153 }
shimizuta 22:0ed9de464f40 154 }
shimizuta 22:0ed9de464f40 155 void can_send(int mode, float duty)
shimizuta 22:0ed9de464f40 156 {
shimizuta 22:0ed9de464f40 157 char data[2]= {0};
shimizuta 22:0ed9de464f40 158 int send=mode * 10 + (int)(duty * 10.0);
shimizuta 22:0ed9de464f40 159 data[0]=send & 0b11111111;
shimizuta 22:0ed9de464f40 160 data[1]=hand_mode;
shimizuta 22:0ed9de464f40 161 if(can1.write(CANMessage(0,data,2)))led4=1;
shimizuta 22:0ed9de464f40 162 else led4=0;
shimizuta 22:0ed9de464f40 163 }
shimizuta 26:5fb1aa9cb7f0 164
MazeTaka 34:0a8ae7f92262 165 float Hcsr04BackWithEc(){
MazeTaka 34:0a8ae7f92262 166 hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
MazeTaka 34:0a8ae7f92262 167 FileWrite();
MazeTaka 34:0a8ae7f92262 168 int is_ok = checkUW(hcsr04_val[1], degree0*kDeg2Rad, ec_li.getCount());//ECは内側を一応選んだ
MazeTaka 34:0a8ae7f92262 169 if(is_ok != 1)
MazeTaka 34:0a8ae7f92262 170 {
MazeTaka 34:0a8ae7f92262 171 hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
MazeTaka 34:0a8ae7f92262 172 FileWrite();
MazeTaka 34:0a8ae7f92262 173 }
MazeTaka 34:0a8ae7f92262 174 return hcsr04_val[1];
MazeTaka 34:0a8ae7f92262 175 }
MazeTaka 34:0a8ae7f92262 176 int checkUW(double uwdist,double theta,int eccount){ //uwdist[cm],theta[rad],eccount[]
MazeTaka 34:0a8ae7f92262 177 const double oneloopdist=0.22f;//[m]
MazeTaka 34:0a8ae7f92262 178 const double ecsliprate=0.7f;
MazeTaka 34:0a8ae7f92262 179 const double errrange=0.5;//収束判定[m]
MazeTaka 34:0a8ae7f92262 180 //const double D=10;//機体中心から足まで距離[m]
MazeTaka 34:0a8ae7f92262 181 static double X=0,Y=0; //初期位置を代入
MazeTaka 34:0a8ae7f92262 182 static double pre_theta=M_pi*0.5f,pre_eccount=0;
MazeTaka 34:0a8ae7f92262 183 theta+=M_pi*0.5f;
MazeTaka 34:0a8ae7f92262 184 double d_eccount=eccount-pre_eccount;
MazeTaka 34:0a8ae7f92262 185 double d_theta=theta-pre_theta;
MazeTaka 34:0a8ae7f92262 186 double ecdist=oneloopdist*d_eccount/1000*ecsliprate;
MazeTaka 34:0a8ae7f92262 187 double rho=ecdist/d_theta;
MazeTaka 34:0a8ae7f92262 188 double dx=-rho*(sin(pre_theta)-sin(theta));//[m]
MazeTaka 34:0a8ae7f92262 189 double dy= rho*(cos(pre_theta)-cos(theta));//[m]
MazeTaka 34:0a8ae7f92262 190 X+=dx;//[m]
MazeTaka 34:0a8ae7f92262 191 Y+=dy;//[m]
MazeTaka 34:0a8ae7f92262 192 pre_theta=theta;
MazeTaka 34:0a8ae7f92262 193 pre_eccount=eccount;
MazeTaka 34:0a8ae7f92262 194 if(fabs(Y-uwdist*0.01f)<errrange)return 1;//Y-uwdist*0.01f*sin(theta)
MazeTaka 34:0a8ae7f92262 195 else return 0;
MazeTaka 34:0a8ae7f92262 196 }