ROBOSTEP_5期 / Mbed 2 deprecated George_Master_BOTHMOVE

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@33:2dbbe198adaf, 2019-05-13 (annotated)

Committer:
yuto17320508
Date:
Mon May 13 09:02:19 2019 +0000
Revision:
33:2dbbe198adaf
Parent:
32:aee87dcaf7ca
Child:
34:0a8ae7f92262
p

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);
shimizuta 22:0ed9de464f40 19
shimizuta 26:5fb1aa9cb7f0 20 //ec
shimizuta 22:0ed9de464f40 21 const int kResolution=500;
shimizuta 22:0ed9de464f40 22 Ec ec_lo(pin_ec[0][0],pin_ec[0][1],NC,kResolution,0.01);
shimizuta 22:0ed9de464f40 23 Ec ec_li(pin_ec[1][0],pin_ec[1][1],NC,kResolution,0.01);
shimizuta 22:0ed9de464f40 24
shimizuta 26:5fb1aa9cb7f0 25 //スイッチ
shimizuta 22:0ed9de464f40 26 DigitalIn bus_in(pin_bus);
shimizuta 22:0ed9de464f40 27 DigitalIn hand(pin_hand,PullUp);
shimizuta 22:0ed9de464f40 28 DigitalIn switch_lo(pin_switch_lo,PullUp);
shimizuta 22:0ed9de464f40 29 DigitalIn switch_li(pin_switch_li,PullUp);
yuto17320508 33:2dbbe198adaf 30 InterruptIn switch_LR(pin_switch_LR,PullUp);
yuto17320508 33:2dbbe198adaf 31 DigitalIn switch_modes[3]= {
yuto17320508 33:2dbbe198adaf 32 DigitalIn(pin_switch_modes[0],PullUp),
yuto17320508 33:2dbbe198adaf 33 DigitalIn(pin_switch_modes[1],PullUp),
yuto17320508 33:2dbbe198adaf 34 DigitalIn(pin_switch_modes[2],PullUp),
yuto17320508 32:aee87dcaf7ca 35 };
yuto17320508 32:aee87dcaf7ca 36 //InterruptIn mode4(pin_mode4,PullUp);
shimizuta 22:0ed9de464f40 37 DigitalOut led4(LED4);
shimizuta 22:0ed9de464f40 38 int hand_mode=NORMAL;
shimizuta 21:14133581387b 39
shimizuta 26:5fb1aa9cb7f0 40 //can
shimizuta 22:0ed9de464f40 41 CAN can1(pin_can_rd,pin_can_td);
shimizuta 22:0ed9de464f40 42
shimizuta 26:5fb1aa9cb7f0 43 //ticker
shimizuta 22:0ed9de464f40 44 Ticker ticker;
shimizuta 26:5fb1aa9cb7f0 45 //const float kTicker_s = 0.2;
shimizuta 26:5fb1aa9cb7f0 46 void Hcsr04Start();//超音波をtickerでとる用
shimizuta 26:5fb1aa9cb7f0 47
shimizuta 26:5fb1aa9cb7f0 48
shimizuta 26:5fb1aa9cb7f0 49 //file操作
shimizuta 26:5fb1aa9cb7f0 50 FILE *fp = NULL;
shimizuta 26:5fb1aa9cb7f0 51 LocalFileSystem local("local");
shimizuta 26:5fb1aa9cb7f0 52 const char kFileName[] = "/local/sensors.csv";
shimizuta 26:5fb1aa9cb7f0 53 Timer filetimer;
shimizuta 26:5fb1aa9cb7f0 54
yuto17320508 33:2dbbe198adaf 55 int FileOpen() //1:異常終了
shimizuta 26:5fb1aa9cb7f0 56 {
shimizuta 27:d392a95f4799 57 printf("file open\r\n");
shimizuta 26:5fb1aa9cb7f0 58 if ((fp = fopen(kFileName, "w")) == NULL) {
shimizuta 26:5fb1aa9cb7f0 59 printf("error : FileSave()\r\n");
shimizuta 26:5fb1aa9cb7f0 60 return 1;
shimizuta 26:5fb1aa9cb7f0 61 }
shimizuta 26:5fb1aa9cb7f0 62 fprintf(fp, "time[s], gyro[deg], hcsr04_forward[cm], hcsr04_back[cm], motor_lo[deg], motor_li[deg]\r\n");
shimizuta 26:5fb1aa9cb7f0 63 filetimer.reset();
shimizuta 26:5fb1aa9cb7f0 64 filetimer.start();
yuto17320508 33:2dbbe198adaf 65 if(RightOrLeft) {
yuto17320508 33:2dbbe198adaf 66 //rightなら
yuto17320508 33:2dbbe198adaf 67 switch_LR.fall(FileClose);//left(0)にしたらclose
yuto17320508 33:2dbbe198adaf 68 } else {
yuto17320508 33:2dbbe198adaf 69 //left
yuto17320508 33:2dbbe198adaf 70 switch_LR.rise(FileClose);//left(0)にしたらclose
yuto17320508 33:2dbbe198adaf 71 }
shimizuta 26:5fb1aa9cb7f0 72 return 0;
shimizuta 26:5fb1aa9cb7f0 73 }
shimizuta 26:5fb1aa9cb7f0 74
shimizuta 26:5fb1aa9cb7f0 75 void FileWrite()
shimizuta 26:5fb1aa9cb7f0 76 {
shimizuta 26:5fb1aa9cb7f0 77 static int is_first = 0;//初回はtimerを0にする
shimizuta 26:5fb1aa9cb7f0 78 if(is_first ==0) {
shimizuta 26:5fb1aa9cb7f0 79 filetimer.reset();
shimizuta 26:5fb1aa9cb7f0 80 ++is_first;
shimizuta 26:5fb1aa9cb7f0 81 }
shimizuta 26:5fb1aa9cb7f0 82 if(fp == NULL) {
shimizuta 26:5fb1aa9cb7f0 83 is_first = 0;
shimizuta 26:5fb1aa9cb7f0 84 } else {
shimizuta 26:5fb1aa9cb7f0 85 fprintf(fp, "%f, %f, %f, %f, %f, %f\r\n",
shimizuta 26:5fb1aa9cb7f0 86 filetimer.read(), degree0,
shimizuta 26:5fb1aa9cb7f0 87 hcsr04_val[0], hcsr04_val[1],
shimizuta 26:5fb1aa9cb7f0 88 motor_lo.getPosi(), motor_li.getPosi());
shimizuta 26:5fb1aa9cb7f0 89 }
shimizuta 26:5fb1aa9cb7f0 90 }
shimizuta 26:5fb1aa9cb7f0 91 void FileClose()
shimizuta 26:5fb1aa9cb7f0 92 {
shimizuta 26:5fb1aa9cb7f0 93 filetimer.stop();
shimizuta 26:5fb1aa9cb7f0 94 if(fp != NULL)
shimizuta 26:5fb1aa9cb7f0 95 fclose(fp);
yuto17320508 33:2dbbe198adaf 96 };
yuto17320508 33:2dbbe198adaf 97
shimizuta 26:5fb1aa9cb7f0 98
shimizuta 22:0ed9de464f40 99
shimizuta 22:0ed9de464f40 100 void TickerSetUp()
shimizuta 22:0ed9de464f40 101 {
shimizuta 26:5fb1aa9cb7f0 102 // ticker.attach(FileWriteTicker,kTicker_s);
shimizuta 22:0ed9de464f40 103 }
shimizuta 22:0ed9de464f40 104
shimizuta 22:0ed9de464f40 105 void Hcsr04Start()
shimizuta 22:0ed9de464f40 106 {
shimizuta 22:0ed9de464f40 107 GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 22:0ed9de464f40 108 GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 22:0ed9de464f40 109 }
shimizuta 21:14133581387b 110 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
shimizuta 21:14133581387b 111 {
shimizuta 27:d392a95f4799 112 wait_ms(90);//60
shimizuta 21:14133581387b 113 sensor.start();
shimizuta 22:0ed9de464f40 114 wait_ms(30);
shimizuta 21:14133581387b 115 float raw_data = sensor.get_dist_cm();
shimizuta 22:0ed9de464f40 116 if(raw_data < 20)//0.1以下なら前回の値を返す
shimizuta 21:14133581387b 117 return kOutVal;
shimizuta 26:5fb1aa9cb7f0 118 /* filter.SetData(raw_data);
shimizuta 26:5fb1aa9cb7f0 119 float dist = filter.GetData();
shimizuta 26:5fb1aa9cb7f0 120 // printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
shimizuta 26:5fb1aa9cb7f0 121 return dist;
shimizuta 26:5fb1aa9cb7f0 122 */
yuto17320508 25:c740e6fd5dab 123 return raw_data;
shimizuta 21:14133581387b 124 }
shimizuta 21:14133581387b 125
shimizuta 22:0ed9de464f40 126 float get_dist_forward()
shimizuta 21:14133581387b 127 {
shimizuta 26:5fb1aa9cb7f0 128 hcsr04_val[0] = GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 26:5fb1aa9cb7f0 129 return hcsr04_val[0];
shimizuta 21:14133581387b 130 }
shimizuta 22:0ed9de464f40 131 float get_dist_back()
shimizuta 21:14133581387b 132 {
shimizuta 26:5fb1aa9cb7f0 133 hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 26:5fb1aa9cb7f0 134 return hcsr04_val[1];
shimizuta 21:14133581387b 135 }
shimizuta 22:0ed9de464f40 136
shimizuta 22:0ed9de464f40 137 void set_gyro()
shimizuta 22:0ed9de464f40 138 {
shimizuta 22:0ed9de464f40 139 device.baud(115200);
shimizuta 22:0ed9de464f40 140 device.format(8,Serial::None,1);
shimizuta 22:0ed9de464f40 141 device.attach(dev_rx, Serial::RxIrq);
shimizuta 22:0ed9de464f40 142 wait(0.05);
shimizuta 22:0ed9de464f40 143 theta0=degree0;
shimizuta 22:0ed9de464f40 144 check_gyro();
shimizuta 22:0ed9de464f40 145 }
shimizuta 22:0ed9de464f40 146 void wait_gerege()
shimizuta 22:0ed9de464f40 147 {
shimizuta 22:0ed9de464f40 148 int i = 0;
shimizuta 22:0ed9de464f40 149 while(i!=100) {
shimizuta 22:0ed9de464f40 150 if(hand.read()==0)i++;
shimizuta 22:0ed9de464f40 151 }
shimizuta 22:0ed9de464f40 152 }
shimizuta 22:0ed9de464f40 153 void can_send(int mode, float duty)
shimizuta 22:0ed9de464f40 154 {
shimizuta 22:0ed9de464f40 155 char data[2]= {0};
shimizuta 22:0ed9de464f40 156 int send=mode * 10 + (int)(duty * 10.0);
shimizuta 22:0ed9de464f40 157 data[0]=send & 0b11111111;
shimizuta 22:0ed9de464f40 158 data[1]=hand_mode;
shimizuta 22:0ed9de464f40 159 if(can1.write(CANMessage(0,data,2)))led4=1;
shimizuta 22:0ed9de464f40 160 else led4=0;
shimizuta 22:0ed9de464f40 161 }
shimizuta 26:5fb1aa9cb7f0 162