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@27:d392a95f4799, 2019-05-08 (annotated)

Committer:
shimizuta
Date:
Wed May 08 02:55:40 2019 +0000
Revision:
27:d392a95f4799
Parent:
26:5fb1aa9cb7f0
Child:
32:aee87dcaf7ca
debuging

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 27:d392a95f4799 49 printf("file open\r\n");
shimizuta 26:5fb1aa9cb7f0 50 if ((fp = fopen(kFileName, "w")) == NULL) {
shimizuta 26:5fb1aa9cb7f0 51 printf("error : FileSave()\r\n");
shimizuta 26:5fb1aa9cb7f0 52 return 1;
shimizuta 26:5fb1aa9cb7f0 53 }
shimizuta 26:5fb1aa9cb7f0 54 fprintf(fp, "time[s], gyro[deg], hcsr04_forward[cm], hcsr04_back[cm], motor_lo[deg], motor_li[deg]\r\n");
shimizuta 26:5fb1aa9cb7f0 55 filetimer.reset();
shimizuta 26:5fb1aa9cb7f0 56 filetimer.start();
shimizuta 26:5fb1aa9cb7f0 57 mode4.rise(FileClose);//mode4ピンを上げげればfileclose
shimizuta 26:5fb1aa9cb7f0 58 return 0;
shimizuta 26:5fb1aa9cb7f0 59 }
shimizuta 26:5fb1aa9cb7f0 60
shimizuta 26:5fb1aa9cb7f0 61 void FileWrite()
shimizuta 26:5fb1aa9cb7f0 62 {
shimizuta 26:5fb1aa9cb7f0 63 static int is_first = 0;//初回はtimerを0にする
shimizuta 26:5fb1aa9cb7f0 64 if(is_first ==0) {
shimizuta 26:5fb1aa9cb7f0 65 filetimer.reset();
shimizuta 26:5fb1aa9cb7f0 66 ++is_first;
shimizuta 26:5fb1aa9cb7f0 67 }
shimizuta 26:5fb1aa9cb7f0 68 if(fp == NULL) {
shimizuta 26:5fb1aa9cb7f0 69 is_first = 0;
shimizuta 26:5fb1aa9cb7f0 70 } else {
shimizuta 26:5fb1aa9cb7f0 71 fprintf(fp, "%f, %f, %f, %f, %f, %f\r\n",
shimizuta 26:5fb1aa9cb7f0 72 filetimer.read(), degree0,
shimizuta 26:5fb1aa9cb7f0 73 hcsr04_val[0], hcsr04_val[1],
shimizuta 26:5fb1aa9cb7f0 74 motor_lo.getPosi(), motor_li.getPosi());
shimizuta 26:5fb1aa9cb7f0 75 }
shimizuta 26:5fb1aa9cb7f0 76 }
shimizuta 26:5fb1aa9cb7f0 77 void FileClose()
shimizuta 26:5fb1aa9cb7f0 78 {
shimizuta 26:5fb1aa9cb7f0 79 filetimer.stop();
shimizuta 26:5fb1aa9cb7f0 80 if(fp != NULL)
shimizuta 26:5fb1aa9cb7f0 81 fclose(fp);
shimizuta 26:5fb1aa9cb7f0 82 };
shimizuta 26:5fb1aa9cb7f0 83
shimizuta 22:0ed9de464f40 84
shimizuta 22:0ed9de464f40 85 void TickerSetUp()
shimizuta 22:0ed9de464f40 86 {
shimizuta 26:5fb1aa9cb7f0 87 // ticker.attach(FileWriteTicker,kTicker_s);
shimizuta 22:0ed9de464f40 88 }
shimizuta 22:0ed9de464f40 89
shimizuta 22:0ed9de464f40 90 void Hcsr04Start()
shimizuta 22:0ed9de464f40 91 {
shimizuta 22:0ed9de464f40 92 GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 22:0ed9de464f40 93 GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 22:0ed9de464f40 94 }
shimizuta 21:14133581387b 95 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
shimizuta 21:14133581387b 96 {
shimizuta 27:d392a95f4799 97 wait_ms(90);//60
shimizuta 21:14133581387b 98 sensor.start();
shimizuta 22:0ed9de464f40 99 wait_ms(30);
shimizuta 21:14133581387b 100 float raw_data = sensor.get_dist_cm();
shimizuta 22:0ed9de464f40 101 if(raw_data < 20)//0.1以下なら前回の値を返す
shimizuta 21:14133581387b 102 return kOutVal;
shimizuta 26:5fb1aa9cb7f0 103 /* filter.SetData(raw_data);
shimizuta 26:5fb1aa9cb7f0 104 float dist = filter.GetData();
shimizuta 26:5fb1aa9cb7f0 105 // printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
shimizuta 26:5fb1aa9cb7f0 106 return dist;
shimizuta 26:5fb1aa9cb7f0 107 */
yuto17320508 25:c740e6fd5dab 108 return raw_data;
shimizuta 21:14133581387b 109 }
shimizuta 21:14133581387b 110
shimizuta 22:0ed9de464f40 111 float get_dist_forward()
shimizuta 21:14133581387b 112 {
shimizuta 26:5fb1aa9cb7f0 113 hcsr04_val[0] = GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
shimizuta 26:5fb1aa9cb7f0 114 return hcsr04_val[0];
shimizuta 21:14133581387b 115 }
shimizuta 22:0ed9de464f40 116 float get_dist_back()
shimizuta 21:14133581387b 117 {
shimizuta 26:5fb1aa9cb7f0 118 hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
shimizuta 26:5fb1aa9cb7f0 119 return hcsr04_val[1];
shimizuta 21:14133581387b 120 }
shimizuta 22:0ed9de464f40 121
shimizuta 22:0ed9de464f40 122 void set_gyro()
shimizuta 22:0ed9de464f40 123 {
shimizuta 22:0ed9de464f40 124 device.baud(115200);
shimizuta 22:0ed9de464f40 125 device.format(8,Serial::None,1);
shimizuta 22:0ed9de464f40 126 device.attach(dev_rx, Serial::RxIrq);
shimizuta 22:0ed9de464f40 127 wait(0.05);
shimizuta 22:0ed9de464f40 128 theta0=degree0;
shimizuta 22:0ed9de464f40 129 check_gyro();
shimizuta 22:0ed9de464f40 130 }
shimizuta 22:0ed9de464f40 131 void wait_gerege()
shimizuta 22:0ed9de464f40 132 {
shimizuta 22:0ed9de464f40 133 int i = 0;
shimizuta 22:0ed9de464f40 134 while(i!=100) {
shimizuta 22:0ed9de464f40 135 if(hand.read()==0)i++;
shimizuta 22:0ed9de464f40 136 }
shimizuta 22:0ed9de464f40 137 }
shimizuta 22:0ed9de464f40 138 void can_send(int mode, float duty)
shimizuta 22:0ed9de464f40 139 {
shimizuta 22:0ed9de464f40 140 char data[2]= {0};
shimizuta 22:0ed9de464f40 141 int send=mode * 10 + (int)(duty * 10.0);
shimizuta 22:0ed9de464f40 142 data[0]=send & 0b11111111;
shimizuta 22:0ed9de464f40 143 data[1]=hand_mode;
shimizuta 22:0ed9de464f40 144 if(can1.write(CANMessage(0,data,2)))led4=1;
shimizuta 22:0ed9de464f40 145 else led4=0;
shimizuta 22:0ed9de464f40 146 }
shimizuta 26:5fb1aa9cb7f0 147