wheel_test7
Dependencies: QEI omni_wheel prototype01 PID ikarashiMDC OmniPosition
main.cpp@1:93f6f64ac719, 2019-08-26 (annotated)
- Committer:
- piroro4560
- Date:
- Mon Aug 26 05:16:15 2019 +0000
- Revision:
- 1:93f6f64ac719
- Parent:
- 0:898471aea1af
using proto01; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 0:898471aea1af | 1 | #include "mbed.h" //wheel_test7 |
piroro4560 | 1:93f6f64ac719 | 2 | #include "ikarashiMDC.h" //protoライブラリ+PID -> うひょひょひょ |
piroro4560 | 0:898471aea1af | 3 | #include "omni_wheel.h" |
piroro4560 | 1:93f6f64ac719 | 4 | #include "proto01.h" |
piroro4560 | 0:898471aea1af | 5 | #include "OmniPosition.h" |
piroro4560 | 0:898471aea1af | 6 | #include "PID.h" |
piroro4560 | 0:898471aea1af | 7 | |
piroro4560 | 0:898471aea1af | 8 | PID angle(2.0,0.0,0.00001,0.01); |
piroro4560 | 0:898471aea1af | 9 | OmniWheel omni(4); |
piroro4560 | 0:898471aea1af | 10 | OmniPosition posi(PC_12, PD_2); |
piroro4560 | 0:898471aea1af | 11 | Serial serial(PC_6, PC_7, 115200); |
piroro4560 | 0:898471aea1af | 12 | DigitalOut serialcontrol(D10); |
piroro4560 | 0:898471aea1af | 13 | RawSerial pc(USBTX, USBRX, 115200); |
piroro4560 | 0:898471aea1af | 14 | DigitalOut sw1(PC_2); |
piroro4560 | 0:898471aea1af | 15 | DigitalOut sw2(PC_3); |
piroro4560 | 0:898471aea1af | 16 | |
piroro4560 | 0:898471aea1af | 17 | ikarashiMDC motor[]= |
piroro4560 | 0:898471aea1af | 18 | { |
piroro4560 | 0:898471aea1af | 19 | ikarashiMDC(&serialcontrol,0,0,SM,&serial), |
piroro4560 | 0:898471aea1af | 20 | ikarashiMDC(&serialcontrol,0,1,SM,&serial), |
piroro4560 | 0:898471aea1af | 21 | ikarashiMDC(&serialcontrol,0,2,SM,&serial), |
piroro4560 | 0:898471aea1af | 22 | ikarashiMDC(&serialcontrol,0,3,SM,&serial), |
piroro4560 | 0:898471aea1af | 23 | }; |
piroro4560 | 0:898471aea1af | 24 | |
piroro4560 | 1:93f6f64ac719 | 25 | Proto1 proto(500, 1000, 0.4, 0.14); |
piroro4560 | 0:898471aea1af | 26 | |
piroro4560 | 0:898471aea1af | 27 | int main() |
piroro4560 | 0:898471aea1af | 28 | { |
piroro4560 | 0:898471aea1af | 29 | sw1 = 1; |
piroro4560 | 0:898471aea1af | 30 | sw2 = 2; |
piroro4560 | 0:898471aea1af | 31 | int cnt=0; |
piroro4560 | 0:898471aea1af | 32 | motor[0].braking = true; |
piroro4560 | 0:898471aea1af | 33 | double valueX, valueY, value[4]; |
piroro4560 | 0:898471aea1af | 34 | omni.wheel[0].setRadian(PI * 1 / 4); |
piroro4560 | 0:898471aea1af | 35 | omni.wheel[1].setRadian(PI * 3 / 4); |
piroro4560 | 0:898471aea1af | 36 | omni.wheel[2].setRadian(PI * 5 / 4); |
piroro4560 | 0:898471aea1af | 37 | omni.wheel[3].setRadian(PI * 7 / 4); |
piroro4560 | 0:898471aea1af | 38 | angle.setInputLimits(-3.14,3.14); |
piroro4560 | 0:898471aea1af | 39 | angle.setOutputLimits(-0.4,0.4); |
piroro4560 | 0:898471aea1af | 40 | angle.setBias(0); |
piroro4560 | 0:898471aea1af | 41 | angle.setMode(1); |
piroro4560 | 0:898471aea1af | 42 | angle.setSetPoint(0); |
piroro4560 | 0:898471aea1af | 43 | while(1){ |
piroro4560 | 0:898471aea1af | 44 | |
piroro4560 | 1:93f6f64ac719 | 45 | if (cnt==500) prot.target_xy(-3800, 0, posi.getX(), posi.getY()); |
piroro4560 | 1:93f6f64ac719 | 46 | if (cnt==1000) proto.target_xy(0, 0, posi.getX(), posi.getY()); |
piroro4560 | 1:93f6f64ac719 | 47 | if (cnt==1500) proto.target_xy(-2000, 0, posi.getX(), posi.getY()); |
piroro4560 | 1:93f6f64ac719 | 48 | if (cnt==2000) proto.target_xy(-2000, 2700, posi.getX(), posi.getY()); |
piroro4560 | 1:93f6f64ac719 | 49 | if (cnt==2500) proto.target_xy(-2000, 0, posi.getX(), posi.getY()); |
piroro4560 | 1:93f6f64ac719 | 50 | if (cnt==3000) proto.target_xy(0, 0, posi.getX(), posi.getY()); |
piroro4560 | 0:898471aea1af | 51 | angle.setProcessValue(posi.getTheta()); |
piroro4560 | 0:898471aea1af | 52 | double spin_power = -angle.compute(); |
piroro4560 | 1:93f6f64ac719 | 53 | proto.calculate(posi.getX(), posi.getY()); |
piroro4560 | 1:93f6f64ac719 | 54 | omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power); |
piroro4560 | 0:898471aea1af | 55 | |
piroro4560 | 0:898471aea1af | 56 | for(int i = 0; i < 4; i++){ |
piroro4560 | 0:898471aea1af | 57 | value[i] = omni.wheel[i]; |
piroro4560 | 0:898471aea1af | 58 | pc.printf("%.2f || ",value[i]); |
piroro4560 | 0:898471aea1af | 59 | } |
piroro4560 | 0:898471aea1af | 60 | pc.printf("x:%d || y:%d || θ:%f || vector:%f || now:%f || c:%f" |
piroro4560 | 1:93f6f64ac719 | 61 | , posi.getX(), posi.getY(), posi.getTheta(), proto.vector, proto.nowDis, proto.counter); |
piroro4560 | 0:898471aea1af | 62 | |
piroro4560 | 0:898471aea1af | 63 | for(int i = 0; i < 4; i++) { |
piroro4560 | 0:898471aea1af | 64 | if (fabs(value[i]) < 0.05) value[i] = 0; |
piroro4560 | 0:898471aea1af | 65 | motor[i].setSpeed(value[i]); |
piroro4560 | 0:898471aea1af | 66 | } |
piroro4560 | 0:898471aea1af | 67 | cnt++; |
piroro4560 | 0:898471aea1af | 68 | pc.printf("||pid:%.2f\r\n",spin_power); |
piroro4560 | 0:898471aea1af | 69 | } |
piroro4560 | 0:898471aea1af | 70 | } |