Ryu Anzawa / Mbed OS wheel_test4

wheel_test4_ver1

Dependencies:   QEI omni_wheel PID R1370 Controller ikarashiMDC

main.cpp@2:8d16db0d55b7, 2019-05-21 (annotated)

Committer:
piroro4560
Date:
Tue May 21 08:39:27 2019 +0000
Revision:
2:8d16db0d55b7
Parent:
1:5c0b7355adcd
Child:
3:4fe8e6e455d0
ver_1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 2:8d16db0d55b7 1 #include "mbed.h" //wheel_test4
piroro4560 0:46c70e5b719d 2 #include "controller.h"
piroro4560 0:46c70e5b719d 3 #include "ikarashiMDC.h"
piroro4560 0:46c70e5b719d 4 #include "R1370.h"
piroro4560 2:8d16db0d55b7 5 #include "omni_wheel.h"
piroro4560 2:8d16db0d55b7 6 #include "PID.h"
piroro4560 0:46c70e5b719d 7 #define PI 3.141593
piroro4560 0:46c70e5b719d 8
piroro4560 2:8d16db0d55b7 9 PID pid1(4.5,100.0,0.00000,0.01);
piroro4560 2:8d16db0d55b7 10 OmniWheel omni(4);
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 2:8d16db0d55b7 16 //DigitalOut reset(PC_9);
piroro4560 2:8d16db0d55b7 17
piroro4560 0:46c70e5b719d 18
skouki 1:5c0b7355adcd 19 ikarashiMDC motor[]=
piroro4560 0:46c70e5b719d 20 {
piroro4560 0:46c70e5b719d 21 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
piroro4560 0:46c70e5b719d 22 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
piroro4560 2:8d16db0d55b7 23 ikarashiMDC(&serialcontrol,2,2,SM,&serial),
piroro4560 2:8d16db0d55b7 24 ikarashiMDC(&serialcontrol,2,3,SM,&serial),
piroro4560 0:46c70e5b719d 25 };
piroro4560 0:46c70e5b719d 26
piroro4560 2:8d16db0d55b7 27
piroro4560 0:46c70e5b719d 28 int main()
piroro4560 0:46c70e5b719d 29 {
piroro4560 2:8d16db0d55b7 30 // reset = 1;
piroro4560 2:8d16db0d55b7 31 motor[0].braking = true;
piroro4560 2:8d16db0d55b7 32 int b[11];
piroro4560 2:8d16db0d55b7 33 double rad[2], dis[2], value[3], X, Y, set_value, original_angle, now_angle, start_angle;
piroro4560 2:8d16db0d55b7 34 pid1.setInputLimits(-180,180);
piroro4560 2:8d16db0d55b7 35 pid1.setOutputLimits(-0.3,0.3);
skouki 1:5c0b7355adcd 36 pid1.setBias(0);
skouki 1:5c0b7355adcd 37 pid1.setMode(1);
piroro4560 2:8d16db0d55b7 38 pid1.setSetPoint(0);////
piroro4560 2:8d16db0d55b7 39 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
piroro4560 2:8d16db0d55b7 40 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
piroro4560 2:8d16db0d55b7 41 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
piroro4560 2:8d16db0d55b7 42 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
piroro4560 0:46c70e5b719d 43 while(1){
piroro4560 0:46c70e5b719d 44 if(pad.receiveState()){
piroro4560 0:46c70e5b719d 45 for(int i = 0; i < 13; i++){
piroro4560 0:46c70e5b719d 46 b[i] = pad.getButton1(i);
piroro4560 0:46c70e5b719d 47 }
piroro4560 0:46c70e5b719d 48 for(int i = 0; i < 2; i++){
piroro4560 0:46c70e5b719d 49 rad[i] = pad.getRadian(i);
piroro4560 0:46c70e5b719d 50 dis[i] = pad.getNorm(i);
piroro4560 0:46c70e5b719d 51 rad[i] = PI - rad[i];
piroro4560 0:46c70e5b719d 52 }
piroro4560 2:8d16db0d55b7 53 X = dis[0] * cos(rad[0]);
piroro4560 2:8d16db0d55b7 54 Y = dis[0] * sin(rad[0]);
piroro4560 0:46c70e5b719d 55 R1370.update();
piroro4560 2:8d16db0d55b7 56 /*
piroro4560 2:8d16db0d55b7 57 ここまでテンプル
piroro4560 2:8d16db0d55b7 58 */
piroro4560 2:8d16db0d55b7 59 start_angle = R1370.getAngle();
piroro4560 2:8d16db0d55b7 60 /*
piroro4560 2:8d16db0d55b7 61 旋回
piroro4560 2:8d16db0d55b7 62 */
piroro4560 2:8d16db0d55b7 63 if(b[7]==0)set_value++;
piroro4560 2:8d16db0d55b7 64 if(b[9]==0)set_value--;
piroro4560 2:8d16db0d55b7 65 set_value %= 4;
piroro4560 2:8d16db0d55b7 66 switch(((set_value & 4) + 4) & 4){
piroro4560 2:8d16db0d55b7 67 case 1: original_angle = 90;break;
piroro4560 2:8d16db0d55b7 68 case 2: original_angle = 179;break;
piroro4560 2:8d16db0d55b7 69 case 3: original_angle = -90;break;
piroro4560 2:8d16db0d55b7 70 default: original_angle = 0;break;
piroro4560 2:8d16db0d55b7 71 /*
piroro4560 2:8d16db0d55b7 72 リセット
piroro4560 2:8d16db0d55b7 73 */
piroro4560 2:8d16db0d55b7 74 if(b[6] == 0){
piroro4560 2:8d16db0d55b7 75 original_angle=angle;
piroro4560 2:8d16db0d55b7 76 }
piroro4560 2:8d16db0d55b7 77 now_angle = start_angle - original_angle;
piroro4560 2:8d16db0d55b7 78 if(now_angle < -180)now_angle = now_angle + 360;
piroro4560 2:8d16db0d55b7 79 pid1.setProcessValue(now_angle);
skouki 1:5c0b7355adcd 80 double spin_power = pid1.compute();
skouki 1:5c0b7355adcd 81 omni.computeXY(X,Y,-spin_power);
piroro4560 2:8d16db0d55b7 82 pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||\n\r",X, Y, start_angle, now_angle, spin_power);
piroro4560 2:8d16db0d55b7 83 for(int i = 0; i < 4; i++)motor[i].setSpeed(omni.wheel[i]);
skouki 1:5c0b7355adcd 84
piroro4560 0:46c70e5b719d 85 }else{
piroro4560 0:46c70e5b719d 86 pc.printf("error\n\r");
piroro4560 2:8d16db0d55b7 87 for (int i = 0; i < 4; i++)motor[i].setSpeed(0);
piroro4560 0:46c70e5b719d 88 }
piroro4560 0:46c70e5b719d 89 }
piroro4560 0:46c70e5b719d 90 }