Ryu Anzawa / Mbed OS wheel_test4

wheel_test4_ver1

Dependencies:   QEI omni_wheel PID R1370 Controller ikarashiMDC

main.cpp@1:5c0b7355adcd, 2019-05-16 (annotated)

Committer:
skouki
Date:
Thu May 16 08:43:30 2019 +0000
Revision:
1:5c0b7355adcd
Parent:
0:46c70e5b719d
Child:
2:8d16db0d55b7
a

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 #define PI 3.141593
skouki 1:5c0b7355adcd 6 #include"omni_wheel.h"
skouki 1:5c0b7355adcd 7 #include"PID.h"
piroro4560 0:46c70e5b719d 8
skouki 1:5c0b7355adcd 9 PID pid1(3.0,3.0,0.000005,0.01);
skouki 1:5c0b7355adcd 10 OmniWheel omni(3);
piroro4560 0:46c70e5b719d 11 Controller pad(PC_10, PC_11, 208);
skouki 1:5c0b7355adcd 12 R1370 R1370(PB_6,PA_10);
piroro4560 0:46c70e5b719d 13 Serial pc(USBTX, USBRX, 115200);
piroro4560 0:46c70e5b719d 14 Serial serial(PC_6, PC_7, 115200);
skouki 1:5c0b7355adcd 15 DigitalOut serialcontrol(D10);
piroro4560 0:46c70e5b719d 16
skouki 1:5c0b7355adcd 17 ikarashiMDC motor[]=
piroro4560 0:46c70e5b719d 18 {
piroro4560 0:46c70e5b719d 19 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
piroro4560 0:46c70e5b719d 20 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
piroro4560 0:46c70e5b719d 21 ikarashiMDC(&serialcontrol,2,2,SM,&serial)
piroro4560 0:46c70e5b719d 22 };
piroro4560 0:46c70e5b719d 23
piroro4560 0:46c70e5b719d 24 int main()
piroro4560 0:46c70e5b719d 25 {
skouki 1:5c0b7355adcd 26 pid1.setInputLimits(-90,90);
skouki 1:5c0b7355adcd 27 pid1.setOutputLimits(-0.5,0.5);
skouki 1:5c0b7355adcd 28 pid1.setBias(0);
skouki 1:5c0b7355adcd 29 pid1.setMode(1);
skouki 1:5c0b7355adcd 30 pid1.setSetPoint(0);
skouki 1:5c0b7355adcd 31 omni.wheel[0].setRadian(PI * 1.0 / 6.0);
skouki 1:5c0b7355adcd 32 omni.wheel[1].setRadian(PI * 5.0 / 6.0);
skouki 1:5c0b7355adcd 33 omni.wheel[2].setRadian(PI * 9.0 / 6.0);
skouki 1:5c0b7355adcd 34
skouki 1:5c0b7355adcd 35 motor[0].braking = true;
piroro4560 0:46c70e5b719d 36 int b[11];
piroro4560 0:46c70e5b719d 37 double rad[2],dis[2];
piroro4560 0:46c70e5b719d 38 double value[3];
skouki 1:5c0b7355adcd 39 pc.printf("saaa");
piroro4560 0:46c70e5b719d 40 while(1){
piroro4560 0:46c70e5b719d 41 if(pad.receiveState()){
piroro4560 0:46c70e5b719d 42 for(int i = 0; i < 13; i++){
piroro4560 0:46c70e5b719d 43 b[i] = pad.getButton1(i);
piroro4560 0:46c70e5b719d 44 }
piroro4560 0:46c70e5b719d 45 for(int i = 0; i < 2; i++){
piroro4560 0:46c70e5b719d 46 rad[i] = pad.getRadian(i);
piroro4560 0:46c70e5b719d 47 dis[i] = pad.getNorm(i);
piroro4560 0:46c70e5b719d 48 rad[i] = PI - rad[i];
piroro4560 0:46c70e5b719d 49 }
skouki 1:5c0b7355adcd 50 double X = dis[0]*cos(rad[0]);
skouki 1:5c0b7355adcd 51 double Y = dis[0]*sin(rad[0]);
piroro4560 0:46c70e5b719d 52 R1370.update();
skouki 1:5c0b7355adcd 53 double angle = R1370.getAngle();
skouki 1:5c0b7355adcd 54 pid1.setProcessValue(angle);
skouki 1:5c0b7355adcd 55 double spin_power = pid1.compute();
skouki 1:5c0b7355adcd 56 omni.computeXY(X,Y,-spin_power);
skouki 1:5c0b7355adcd 57 pc.printf("%.3f||%.3f||%.3f||%.3f\n\r",X,Y,angle,spin_power);
skouki 1:5c0b7355adcd 58 for(int i = 0; i < 3; i++)
skouki 1:5c0b7355adcd 59 motor[i].setSpeed(omni.wheel[i]);
skouki 1:5c0b7355adcd 60
piroro4560 0:46c70e5b719d 61 }else{
piroro4560 0:46c70e5b719d 62 pc.printf("error\n\r");
piroro4560 0:46c70e5b719d 63 for (int i = 0; i < 3; i++)
piroro4560 0:46c70e5b719d 64 {
skouki 1:5c0b7355adcd 65 motor[i].setSpeed(0);
piroro4560 0:46c70e5b719d 66 }
piroro4560 0:46c70e5b719d 67 }
piroro4560 0:46c70e5b719d 68 }
piroro4560 0:46c70e5b719d 69 }