hirromu takaki / Mbed OS 106_MD_practice

動いた

Dependencies:   lib kohiMD lpf

main.cpp@3:f09bb38d0909, 2021-03-04 (annotated)

Committer:
THtakahiro702286
Date:
Thu Mar 04 10:51:22 2021 +0000
Revision:
3:f09bb38d0909
Parent:
1:81190825251c
Child:
4:d0a512e71296
mbed os 6;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 0:2762f6b27c5f 1 #include "mbed.h"
THtakahiro702286 3:f09bb38d0909 2 //#include <math.h>
THtakahiro702286 1:81190825251c 3 #include "kohiMD.h"
THtakahiro702286 0:2762f6b27c5f 4 #include "lpf.h"
THtakahiro702286 3:f09bb38d0909 5 #include "lib.h"
THtakahiro702286 0:2762f6b27c5f 6
THtakahiro702286 3:f09bb38d0909 7 //using namespace std::chrono;
THtakahiro702286 3:f09bb38d0909 8
THtakahiro702286 3:f09bb38d0909 9 Lib lib;
THtakahiro702286 3:f09bb38d0909 10 //KohiMD md[4] = {KohiMD(PA_15), KohiMD(PA_6), KohiMD(PA_7), KohiMD(PB_6)};
THtakahiro702286 3:f09bb38d0909 11 //DigitalOut output[4] = {DigitalOut(PB_6),DigitalOut(PC_7),DigitalOut(PB_5),DigitalOut(PA_10)};
THtakahiro702286 0:2762f6b27c5f 12 //DigitalIn DS[3] = { DigitalIn(PC_12), DigitalIn(PC_10), DigitalIn(PD_2) };
THtakahiro702286 3:f09bb38d0909 13 Timer t;
THtakahiro702286 3:f09bb38d0909 14 //Serial pc(USBTX,USBRX,115200);
THtakahiro702286 0:2762f6b27c5f 15 lpf mdlpf(1.0/300,0.005);
THtakahiro702286 0:2762f6b27c5f 16 //DigitalIn fallut(PB_6);
THtakahiro702286 0:2762f6b27c5f 17 //DigitalIn otw(PC_7);
THtakahiro702286 1:81190825251c 18 DigitalIn b(USER_BUTTON);
THtakahiro702286 1:81190825251c 19 DigitalOut led(LED1);
THtakahiro702286 0:2762f6b27c5f 20
THtakahiro702286 0:2762f6b27c5f 21 int main()
THtakahiro702286 0:2762f6b27c5f 22 {
THtakahiro702286 3:f09bb38d0909 23 t.start();
THtakahiro702286 3:f09bb38d0909 24 int p1,p2,b1=0,b2=0,co=0;
THtakahiro702286 0:2762f6b27c5f 25 double pwm = 0.0,put;
THtakahiro702286 3:f09bb38d0909 26 for(int i=0; i < 4; i++)
THtakahiro702286 3:f09bb38d0909 27 {
THtakahiro702286 3:f09bb38d0909 28 // output[i] = 1;
THtakahiro702286 3:f09bb38d0909 29 }
THtakahiro702286 0:2762f6b27c5f 30 while (true) {
THtakahiro702286 0:2762f6b27c5f 31
THtakahiro702286 3:f09bb38d0909 32 // pwm = sin(duration_cast<milliseconds>(t.elapsed_time()).count() / 1000.0);
THtakahiro702286 3:f09bb38d0909 33
THtakahiro702286 3:f09bb38d0909 34 b1 = b2-b;
THtakahiro702286 3:f09bb38d0909 35 b2 = b;
THtakahiro702286 3:f09bb38d0909 36 if(b1 == 1)
THtakahiro702286 3:f09bb38d0909 37 {
THtakahiro702286 3:f09bb38d0909 38 co++;
THtakahiro702286 3:f09bb38d0909 39 }
THtakahiro702286 3:f09bb38d0909 40 pwm = (co % 10) / 10.0 - 0.5;
THtakahiro702286 3:f09bb38d0909 41
THtakahiro702286 3:f09bb38d0909 42
THtakahiro702286 3:f09bb38d0909 43
THtakahiro702286 3:f09bb38d0909 44
THtakahiro702286 0:2762f6b27c5f 45 for(int i=0; i < 4; i++)
THtakahiro702286 0:2762f6b27c5f 46 {
THtakahiro702286 1:81190825251c 47 if(pwm > 0.5) pwm = 0.5;
THtakahiro702286 1:81190825251c 48 if(pwm < -0.5) pwm = -0.5;
THtakahiro702286 3:f09bb38d0909 49 // if(b) pwm = 0;
THtakahiro702286 3:f09bb38d0909 50 // md[i].setSpeed(pwm);
THtakahiro702286 1:81190825251c 51 }
THtakahiro702286 1:81190825251c 52 led = b;
THtakahiro702286 3:f09bb38d0909 53 // mdlpf.path_value(pwm);
THtakahiro702286 0:2762f6b27c5f 54 // pwm += ((pwm < 0.1) - (pwm > 0.9)) * 0.1;
THtakahiro702286 1:81190825251c 55 put = 0.5 + pwm / 2.0;
THtakahiro702286 1:81190825251c 56 if(put > 0.9) put = 0.9;
THtakahiro702286 1:81190825251c 57 if(put < 0.1) put = 0.1;
THtakahiro702286 1:81190825251c 58 // put = pwm * 0.9 / 2 + 0.5;
THtakahiro702286 1:81190825251c 59 //pc.printf("%f \n\r",put);
THtakahiro702286 3:f09bb38d0909 60 lib.go(0.0);
THtakahiro702286 3:f09bb38d0909 61 p1 = pwm * 100;
THtakahiro702286 3:f09bb38d0909 62 p2 = put * 100;
THtakahiro702286 3:f09bb38d0909 63 printf("%d %d %d %d %d\r\n",p1,p2,(int)b,b1,b2);
THtakahiro702286 3:f09bb38d0909 64 // printf("The time taken was %llu milliseconds\n", duration_cast<milliseconds>(t.elapsed_time()).count());
THtakahiro702286 0:2762f6b27c5f 65 }
THtakahiro702286 0:2762f6b27c5f 66 }
THtakahiro702286 0:2762f6b27c5f 67