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@26:5fb1aa9cb7f0, 2019-05-06 (annotated)

Committer:
shimizuta
Date:
Mon May 06 06:28:41 2019 +0000
Revision:
26:5fb1aa9cb7f0
Parent:
25:c740e6fd5dab
Child:
27:d392a95f4799
can get log

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