Ryu Anzawa / Mbed OS wheel_test4

wheel_test4_ver1

Dependencies:   QEI omni_wheel PID R1370 Controller ikarashiMDC

main.cpp@0:46c70e5b719d, 2019-05-16 (annotated)

Committer:
piroro4560
Date:
Thu May 16 07:41:44 2019 +0000
Revision:
0:46c70e5b719d
Child:
1:5c0b7355adcd
wheeltest3;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 0:46c70e5b719d 1 #include "mbed.h" //wheel_test3
piroro4560 0:46c70e5b719d 2 #include "controller.h"
piroro4560 0:46c70e5b719d 3 #include "ikarashiMDC.h"
piroro4560 0:46c70e5b719d 4 #include "R1370.h"
piroro4560 0:46c70e5b719d 5 #include "wheel_an.h"
piroro4560 0:46c70e5b719d 6 #define PI 3.141593
piroro4560 0:46c70e5b719d 7
piroro4560 0:46c70e5b719d 8 Controller pad(PC_10, PC_11, 208);
piroro4560 0:46c70e5b719d 9 R1370 R1370(D10,D2);
piroro4560 0:46c70e5b719d 10 Serial pc(USBTX, USBRX, 115200);
piroro4560 0:46c70e5b719d 11 Serial serial(PC_6, PC_7, 115200);
piroro4560 0:46c70e5b719d 12 DigitalOut serialcontrol(PD_2);
piroro4560 0:46c70e5b719d 13
piroro4560 0:46c70e5b719d 14 ikarashiMDC ikarashi[]
piroro4560 0:46c70e5b719d 15 {
piroro4560 0:46c70e5b719d 16 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
piroro4560 0:46c70e5b719d 17 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
piroro4560 0:46c70e5b719d 18 ikarashiMDC(&serialcontrol,2,2,SM,&serial)
piroro4560 0:46c70e5b719d 19 };
piroro4560 0:46c70e5b719d 20
piroro4560 0:46c70e5b719d 21 Wheelan wheel[]
piroro4560 0:46c70e5b719d 22 {
piroro4560 0:46c70e5b719d 23 Wheelan(PI * 1 / 6),
piroro4560 0:46c70e5b719d 24 Wheelan(PI * 5 / 6),
piroro4560 0:46c70e5b719d 25 Wheelan(PI * 9 / 6)
piroro4560 0:46c70e5b719d 26 };
piroro4560 0:46c70e5b719d 27
piroro4560 0:46c70e5b719d 28 int main()
piroro4560 0:46c70e5b719d 29 {
piroro4560 0:46c70e5b719d 30 ikarashi[0].braking = true;
piroro4560 0:46c70e5b719d 31 int b[11];
piroro4560 0:46c70e5b719d 32 double rad[2],dis[2];
piroro4560 0:46c70e5b719d 33 double value[3];
piroro4560 0:46c70e5b719d 34 while(1){
piroro4560 0:46c70e5b719d 35 if(pad.receiveState()){
piroro4560 0:46c70e5b719d 36 for(int i = 0; i < 13; i++){
piroro4560 0:46c70e5b719d 37 b[i] = pad.getButton1(i);
piroro4560 0:46c70e5b719d 38 }
piroro4560 0:46c70e5b719d 39 for(int i = 0; i < 2; i++){
piroro4560 0:46c70e5b719d 40 rad[i] = pad.getRadian(i);
piroro4560 0:46c70e5b719d 41 dis[i] = pad.getNorm(i);
piroro4560 0:46c70e5b719d 42 rad[i] = PI - rad[i];
piroro4560 0:46c70e5b719d 43 }
piroro4560 0:46c70e5b719d 44 R1370.update();
piroro4560 0:46c70e5b719d 45 pc.printf("degree : %.3lf ", R1370.getAngle());
piroro4560 0:46c70e5b719d 46
piroro4560 0:46c70e5b719d 47 for (int i = 0; i < 3; ++i)
piroro4560 0:46c70e5b719d 48 {
piroro4560 0:46c70e5b719d 49 value[i] = wheel[i].an_uc(rad[1], dis[1]);
piroro4560 0:46c70e5b719d 50 }
piroro4560 0:46c70e5b719d 51
piroro4560 0:46c70e5b719d 52 if(b[7] == 1 && b[9] == 0) {
piroro4560 0:46c70e5b719d 53 value[0] -= 0.3;
piroro4560 0:46c70e5b719d 54 value[1] -= 0.3;
piroro4560 0:46c70e5b719d 55 value[2] -= 0.3;
piroro4560 0:46c70e5b719d 56 } else if (b[9] == 1 && b[7] == 0) {
piroro4560 0:46c70e5b719d 57 value[0] += 0.3;
piroro4560 0:46c70e5b719d 58 value[1] += 0.3;
piroro4560 0:46c70e5b719d 59 value[2] += 0.3;
piroro4560 0:46c70e5b719d 60 }
piroro4560 0:46c70e5b719d 61
piroro4560 0:46c70e5b719d 62 for (int i = 0; i < 3; ++i)
piroro4560 0:46c70e5b719d 63 {
piroro4560 0:46c70e5b719d 64 pc.printf("%lf ", value[i]);
piroro4560 0:46c70e5b719d 65 ikarashi[i].setSpeed(value[i]);
piroro4560 0:46c70e5b719d 66 }
piroro4560 0:46c70e5b719d 67 pc.printf("\n\r");
piroro4560 0:46c70e5b719d 68
piroro4560 0:46c70e5b719d 69 }else{
piroro4560 0:46c70e5b719d 70 pc.printf("error\n\r");
piroro4560 0:46c70e5b719d 71 for (int i = 0; i < 3; i++)
piroro4560 0:46c70e5b719d 72 {
piroro4560 0:46c70e5b719d 73 ikarashi[i].setSpeed(0);
piroro4560 0:46c70e5b719d 74 }
piroro4560 0:46c70e5b719d 75 }
piroro4560 0:46c70e5b719d 76 }
piroro4560 0:46c70e5b719d 77 }