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@32:aee87dcaf7ca, 2019-05-13 (annotated)

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