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@22:0ed9de464f40, 2019-05-03 (annotated)

Committer:
shimizuta
Date:
Fri May 03 09:34:05 2019 +0000
Revision:
22:0ed9de464f40
Parent:
21:14133581387b
Child:
25:c740e6fd5dab
a

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 22:0ed9de464f40 4 const float kOldWeight = 0.5;
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 {
shimizuta 22:0ed9de464f40 47 wait_ms(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;
shimizuta 21:14133581387b 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;
shimizuta 21:14133581387b 57 }
shimizuta 21:14133581387b 58
shimizuta 22:0ed9de464f40 59 float get_dist_forward()
shimizuta 21:14133581387b 60 {
shimizuta 21:14133581387b 61 return GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 21:14133581387b 62 }
shimizuta 22:0ed9de464f40 63 float get_dist_back()
shimizuta 21:14133581387b 64 {
shimizuta 21:14133581387b 65 return GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 21:14133581387b 66 }
shimizuta 22:0ed9de464f40 67
shimizuta 22:0ed9de464f40 68 void set_gyro()
shimizuta 22:0ed9de464f40 69 {
shimizuta 22:0ed9de464f40 70 device.baud(115200);
shimizuta 22:0ed9de464f40 71 device.format(8,Serial::None,1);
shimizuta 22:0ed9de464f40 72 device.attach(dev_rx, Serial::RxIrq);
shimizuta 22:0ed9de464f40 73 wait(0.05);
shimizuta 22:0ed9de464f40 74 theta0=degree0;
shimizuta 22:0ed9de464f40 75 check_gyro();
shimizuta 22:0ed9de464f40 76 }
shimizuta 22:0ed9de464f40 77 void wait_gerege()
shimizuta 22:0ed9de464f40 78 {
shimizuta 22:0ed9de464f40 79 int i = 0;
shimizuta 22:0ed9de464f40 80 while(i!=100) {
shimizuta 22:0ed9de464f40 81 if(hand.read()==0)i++;
shimizuta 22:0ed9de464f40 82 }
shimizuta 22:0ed9de464f40 83 }
shimizuta 22:0ed9de464f40 84 void can_send(int mode, float duty)
shimizuta 22:0ed9de464f40 85 {
shimizuta 22:0ed9de464f40 86 char data[2]= {0};
shimizuta 22:0ed9de464f40 87 int send=mode * 10 + (int)(duty * 10.0);
shimizuta 22:0ed9de464f40 88 data[0]=send & 0b11111111;
shimizuta 22:0ed9de464f40 89 data[1]=hand_mode;
shimizuta 22:0ed9de464f40 90 if(can1.write(CANMessage(0,data,2)))led4=1;
shimizuta 22:0ed9de464f40 91 else led4=0;
shimizuta 22:0ed9de464f40 92 }