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@25:c740e6fd5dab, 2019-05-06 (annotated)

Committer:
yuto17320508
Date:
Mon May 06 04:03:00 2019 +0000
Revision:
25:c740e6fd5dab
Parent:
22:0ed9de464f40
Child:
26:5fb1aa9cb7f0
l

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"
yuto17320508 25:c740e6fd5dab 4 const float kOldWeight = 0;
shimizuta 22:0ed9de464f40 5 const float kOldWeightLight = 0.3; //filterの重み.軽いver
shimizuta 22:0ed9de464f40 6
shimizuta 22:0ed9de464f40 7 const float kOutVal = 500; //<0.1が返ってきたときに返す値
shimizuta 21:14133581387b 8 //filter [0]:forward, [1]: back
shimizuta 21:14133581387b 9 LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};
shimizuta 21:14133581387b 10
shimizuta 22:0ed9de464f40 11 HCSR04 sensor_forward(pin_hcsr04[0][0],pin_hcsr04[0][1]);
shimizuta 22:0ed9de464f40 12 HCSR04 sensor_back(pin_hcsr04[1][0], pin_hcsr04[1][1]);
shimizuta 22:0ed9de464f40 13
shimizuta 22:0ed9de464f40 14 const int kResolution=500;
shimizuta 22:0ed9de464f40 15 Ec ec_lo(pin_ec[0][0],pin_ec[0][1],NC,kResolution,0.01);
shimizuta 22:0ed9de464f40 16 Ec ec_li(pin_ec[1][0],pin_ec[1][1],NC,kResolution,0.01);
shimizuta 22:0ed9de464f40 17
shimizuta 22:0ed9de464f40 18
shimizuta 22:0ed9de464f40 19 DigitalIn bus_in(pin_bus);
shimizuta 22:0ed9de464f40 20 DigitalIn hand(pin_hand,PullUp);
shimizuta 22:0ed9de464f40 21 DigitalIn switch_lo(pin_switch_lo,PullUp);
shimizuta 22:0ed9de464f40 22 DigitalIn switch_li(pin_switch_li,PullUp);
shimizuta 22:0ed9de464f40 23 DigitalIn mode4(pin_mode4,PullUp);
shimizuta 22:0ed9de464f40 24 DigitalOut led4(LED4);
shimizuta 22:0ed9de464f40 25
shimizuta 22:0ed9de464f40 26 int hand_mode=NORMAL;
shimizuta 21:14133581387b 27
shimizuta 22:0ed9de464f40 28 CAN can1(pin_can_rd,pin_can_td);
shimizuta 22:0ed9de464f40 29
shimizuta 22:0ed9de464f40 30 Ticker ticker;
shimizuta 22:0ed9de464f40 31 const float kTicker_s = 0.2;
shimizuta 22:0ed9de464f40 32 void Hcsr04Start();
shimizuta 22:0ed9de464f40 33 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);
shimizuta 22:0ed9de464f40 34
shimizuta 22:0ed9de464f40 35 void TickerSetUp()
shimizuta 22:0ed9de464f40 36 {
shimizuta 22:0ed9de464f40 37 ticker.attach(Hcsr04Start,kTicker_s);
shimizuta 22:0ed9de464f40 38 }
shimizuta 22:0ed9de464f40 39
shimizuta 22:0ed9de464f40 40 void Hcsr04Start()
shimizuta 22:0ed9de464f40 41 {
shimizuta 22:0ed9de464f40 42 GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 22:0ed9de464f40 43 GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 22:0ed9de464f40 44 }
shimizuta 21:14133581387b 45 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
shimizuta 21:14133581387b 46 {
yuto17320508 25:c740e6fd5dab 47 wait_ms(30);//60
shimizuta 21:14133581387b 48 sensor.start();
shimizuta 22:0ed9de464f40 49 wait_ms(30);
shimizuta 21:14133581387b 50 float raw_data = sensor.get_dist_cm();
shimizuta 22:0ed9de464f40 51 if(raw_data < 20)//0.1以下なら前回の値を返す
shimizuta 21:14133581387b 52 return kOutVal;
yuto17320508 25:c740e6fd5dab 53 /* filter.SetData(raw_data);
shimizuta 21:14133581387b 54 float dist = filter.GetData();
shimizuta 22:0ed9de464f40 55 // printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
shimizuta 21:14133581387b 56 return dist;
yuto17320508 25:c740e6fd5dab 57 */
yuto17320508 25:c740e6fd5dab 58 return raw_data;
shimizuta 21:14133581387b 59 }
shimizuta 21:14133581387b 60
shimizuta 22:0ed9de464f40 61 float get_dist_forward()
shimizuta 21:14133581387b 62 {
shimizuta 21:14133581387b 63 return GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 21:14133581387b 64 }
shimizuta 22:0ed9de464f40 65 float get_dist_back()
shimizuta 21:14133581387b 66 {
shimizuta 21:14133581387b 67 return GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 21:14133581387b 68 }
shimizuta 22:0ed9de464f40 69
shimizuta 22:0ed9de464f40 70 void set_gyro()
shimizuta 22:0ed9de464f40 71 {
shimizuta 22:0ed9de464f40 72 device.baud(115200);
shimizuta 22:0ed9de464f40 73 device.format(8,Serial::None,1);
shimizuta 22:0ed9de464f40 74 device.attach(dev_rx, Serial::RxIrq);
shimizuta 22:0ed9de464f40 75 wait(0.05);
shimizuta 22:0ed9de464f40 76 theta0=degree0;
shimizuta 22:0ed9de464f40 77 check_gyro();
shimizuta 22:0ed9de464f40 78 }
shimizuta 22:0ed9de464f40 79 void wait_gerege()
shimizuta 22:0ed9de464f40 80 {
shimizuta 22:0ed9de464f40 81 int i = 0;
shimizuta 22:0ed9de464f40 82 while(i!=100) {
shimizuta 22:0ed9de464f40 83 if(hand.read()==0)i++;
shimizuta 22:0ed9de464f40 84 }
shimizuta 22:0ed9de464f40 85 }
shimizuta 22:0ed9de464f40 86 void can_send(int mode, float duty)
shimizuta 22:0ed9de464f40 87 {
shimizuta 22:0ed9de464f40 88 char data[2]= {0};
shimizuta 22:0ed9de464f40 89 int send=mode * 10 + (int)(duty * 10.0);
shimizuta 22:0ed9de464f40 90 data[0]=send & 0b11111111;
shimizuta 22:0ed9de464f40 91 data[1]=hand_mode;
shimizuta 22:0ed9de464f40 92 if(can1.write(CANMessage(0,data,2)))led4=1;
shimizuta 22:0ed9de464f40 93 else led4=0;
shimizuta 22:0ed9de464f40 94 }