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?
User | Revision | Line number | New 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 | } |