Ryu Anzawa / Mbed OS wheel_test4

wheel_test4_ver1

Dependencies:   QEI omni_wheel PID R1370 Controller ikarashiMDC

main.cpp@3:4fe8e6e455d0, 2019-06-11 (annotated)

Committer:
piroro4560
Date:
Tue Jun 11 09:10:36 2019 +0000
Revision:
3:4fe8e6e455d0
Parent:
2:8d16db0d55b7 
dekiterukamo

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 3:4fe8e6e455d0 9 PID pid1(5.0,0.001,0.0001,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 3:4fe8e6e455d0 32 int b[11], b2[11];
piroro4560 3:4fe8e6e455d0 33 double rad[2], dis[2], value[3], X, Y, original_angle, now_angle, start_angle, spin_power;
piroro4560 3:4fe8e6e455d0 34 /*
piroro4560 3:4fe8e6e455d0 35 * original_angle :
piroro4560 3:4fe8e6e455d0 36 * now_angle :
piroro4560 3:4fe8e6e455d0 37 * start_angle :
piroro4560 3:4fe8e6e455d0 38 */
piroro4560 2:8d16db0d55b7 39 pid1.setInputLimits(-180,180);
piroro4560 2:8d16db0d55b7 40 pid1.setOutputLimits(-0.3,0.3);
skouki 1:5c0b7355adcd 41 pid1.setBias(0);
skouki 1:5c0b7355adcd 42 pid1.setMode(1);
piroro4560 2:8d16db0d55b7 43 pid1.setSetPoint(0);////
piroro4560 2:8d16db0d55b7 44 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
piroro4560 2:8d16db0d55b7 45 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
piroro4560 2:8d16db0d55b7 46 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
piroro4560 2:8d16db0d55b7 47 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
piroro4560 0:46c70e5b719d 48 while(1){
piroro4560 0:46c70e5b719d 49 if(pad.receiveState()){
piroro4560 0:46c70e5b719d 50 for(int i = 0; i < 13; i++){
piroro4560 3:4fe8e6e455d0 51 b[i] = 1 - pad.getButton1(i);
piroro4560 0:46c70e5b719d 52 }
piroro4560 0:46c70e5b719d 53 for(int i = 0; i < 2; i++){
piroro4560 0:46c70e5b719d 54 rad[i] = pad.getRadian(i);
piroro4560 0:46c70e5b719d 55 dis[i] = pad.getNorm(i);
piroro4560 0:46c70e5b719d 56 rad[i] = PI - rad[i];
piroro4560 0:46c70e5b719d 57 }
piroro4560 2:8d16db0d55b7 58 X = dis[0] * cos(rad[0]);
piroro4560 2:8d16db0d55b7 59 Y = dis[0] * sin(rad[0]);
piroro4560 0:46c70e5b719d 60 R1370.update();
piroro4560 3:4fe8e6e455d0 61 /*ここまでテンプル*/
piroro4560 2:8d16db0d55b7 62 start_angle = R1370.getAngle();
piroro4560 2:8d16db0d55b7 63 /*
piroro4560 2:8d16db0d55b7 64 旋回
piroro4560 2:8d16db0d55b7 65 */
piroro4560 2:8d16db0d55b7 66 /*
piroro4560 2:8d16db0d55b7 67 リセット
piroro4560 2:8d16db0d55b7 68 */
piroro4560 3:4fe8e6e455d0 69 if(b[6] == 1){
piroro4560 3:4fe8e6e455d0 70 original_angle=start_angle;
piroro4560 2:8d16db0d55b7 71 }
piroro4560 2:8d16db0d55b7 72 now_angle = start_angle - original_angle;
piroro4560 2:8d16db0d55b7 73 pid1.setProcessValue(now_angle);
piroro4560 3:4fe8e6e455d0 74 spin_power = pid1.compute();
skouki 1:5c0b7355adcd 75 omni.computeXY(X,Y,-spin_power);
piroro4560 2:8d16db0d55b7 76 pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||\n\r",X, Y, start_angle, now_angle, spin_power);
piroro4560 2:8d16db0d55b7 77 for(int i = 0; i < 4; i++)motor[i].setSpeed(omni.wheel[i]);
piroro4560 3:4fe8e6e455d0 78 for(int i = 0; i < 11; ++i)b2[i] = b[i];
piroro4560 0:46c70e5b719d 79 }else{
piroro4560 0:46c70e5b719d 80 pc.printf("error\n\r");
piroro4560 2:8d16db0d55b7 81 for (int i = 0; i < 4; i++)motor[i].setSpeed(0);
piroro4560 0:46c70e5b719d 82 }
piroro4560 0:46c70e5b719d 83 }
piroro4560 0:46c70e5b719d 84 }