wheel_test5 ver_1
Dependencies: QEI omni_wheel PID R1370 Controller ikarashiMDC
main.cpp@0:424b608bab8c, 2019-06-20 (annotated)
- Committer:
- piroro4560
- Date:
- Thu Jun 20 07:03:57 2019 +0000
- Revision:
- 0:424b608bab8c
ver_1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 0:424b608bab8c | 1 | #include "mbed.h" //wheel_test5 |
piroro4560 | 0:424b608bab8c | 2 | #include "controller.h" |
piroro4560 | 0:424b608bab8c | 3 | #include "ikarashiMDC.h" |
piroro4560 | 0:424b608bab8c | 4 | #include "R1370.h" |
piroro4560 | 0:424b608bab8c | 5 | #include "omni_wheel.h" |
piroro4560 | 0:424b608bab8c | 6 | #include "PID.h" |
piroro4560 | 0:424b608bab8c | 7 | #define PI 3.1415926535897932384626 |
piroro4560 | 0:424b608bab8c | 8 | |
piroro4560 | 0:424b608bab8c | 9 | PID pid1(5.0,0.2,0.000095,0.01); |
piroro4560 | 0:424b608bab8c | 10 | OmniWheel omni(4); |
piroro4560 | 0:424b608bab8c | 11 | Controller pad(PC_10, PC_11, 208); |
piroro4560 | 0:424b608bab8c | 12 | R1370 R1370(PB_6,PA_10); |
piroro4560 | 0:424b608bab8c | 13 | Serial pc(USBTX, USBRX, 115200); |
piroro4560 | 0:424b608bab8c | 14 | Serial serial(PC_6, PC_7, 115200); |
piroro4560 | 0:424b608bab8c | 15 | DigitalOut serialcontrol(D10); |
piroro4560 | 0:424b608bab8c | 16 | //DigitalOut reset(PC_9); |
piroro4560 | 0:424b608bab8c | 17 | |
piroro4560 | 0:424b608bab8c | 18 | |
piroro4560 | 0:424b608bab8c | 19 | ikarashiMDC motor[]= |
piroro4560 | 0:424b608bab8c | 20 | { |
piroro4560 | 0:424b608bab8c | 21 | ikarashiMDC(&serialcontrol,2,0,SM,&serial), |
piroro4560 | 0:424b608bab8c | 22 | ikarashiMDC(&serialcontrol,2,1,SM,&serial), |
piroro4560 | 0:424b608bab8c | 23 | ikarashiMDC(&serialcontrol,2,2,SM,&serial), |
piroro4560 | 0:424b608bab8c | 24 | ikarashiMDC(&serialcontrol,2,3,SM,&serial), |
piroro4560 | 0:424b608bab8c | 25 | }; |
piroro4560 | 0:424b608bab8c | 26 | |
piroro4560 | 0:424b608bab8c | 27 | |
piroro4560 | 0:424b608bab8c | 28 | double sign(double a){ |
piroro4560 | 0:424b608bab8c | 29 | return(a>0)-(a<0); |
piroro4560 | 0:424b608bab8c | 30 | } |
piroro4560 | 0:424b608bab8c | 31 | |
piroro4560 | 0:424b608bab8c | 32 | int main() |
piroro4560 | 0:424b608bab8c | 33 | { |
piroro4560 | 0:424b608bab8c | 34 | // reset = 1; |
piroro4560 | 0:424b608bab8c | 35 | motor[0].braking = true; |
piroro4560 | 0:424b608bab8c | 36 | int b[11], b2[11], b3[11], angle_state; |
piroro4560 | 0:424b608bab8c | 37 | double rad[2], dis[2], value[3], X, Y, original_angle=0, now_angle, start_angle, zero=0, spin_power; |
piroro4560 | 0:424b608bab8c | 38 | double deviation = 0.08; |
piroro4560 | 0:424b608bab8c | 39 | /** |
piroro4560 | 0:424b608bab8c | 40 | * now_angle : 今の角度 |
piroro4560 | 0:424b608bab8c | 41 | * start_angle : 素の角度 |
piroro4560 | 0:424b608bab8c | 42 | * original_angle : 0,-90,90,180 |
piroro4560 | 0:424b608bab8c | 43 | * deviation : 差 |
piroro4560 | 0:424b608bab8c | 44 | * angle_state : 90度毎 |
piroro4560 | 0:424b608bab8c | 45 | * zero : 零点合わせ |
piroro4560 | 0:424b608bab8c | 46 | */ |
piroro4560 | 0:424b608bab8c | 47 | pid1.setInputLimits(-180,180); |
piroro4560 | 0:424b608bab8c | 48 | pid1.setOutputLimits(-0.3,0.3); |
piroro4560 | 0:424b608bab8c | 49 | pid1.setBias(0); |
piroro4560 | 0:424b608bab8c | 50 | pid1.setMode(1); |
piroro4560 | 0:424b608bab8c | 51 | pid1.setSetPoint(0);//// |
piroro4560 | 0:424b608bab8c | 52 | omni.wheel[0].setRadian(PI * 1.0 / 4.0); |
piroro4560 | 0:424b608bab8c | 53 | omni.wheel[1].setRadian(PI * 3.0 / 4.0); |
piroro4560 | 0:424b608bab8c | 54 | omni.wheel[2].setRadian(PI * 5.0 / 4.0); |
piroro4560 | 0:424b608bab8c | 55 | omni.wheel[3].setRadian(PI * 7.0 / 4.0); |
piroro4560 | 0:424b608bab8c | 56 | while(1){ |
piroro4560 | 0:424b608bab8c | 57 | if(pad.receiveState()){ |
piroro4560 | 0:424b608bab8c | 58 | for(int i = 0; i < 13; i++){ |
piroro4560 | 0:424b608bab8c | 59 | b[i] = 1 - pad.getButton1(i); |
piroro4560 | 0:424b608bab8c | 60 | } |
piroro4560 | 0:424b608bab8c | 61 | for(int i = 0; i < 2; i++){ |
piroro4560 | 0:424b608bab8c | 62 | rad[i] = pad.getRadian(i); |
piroro4560 | 0:424b608bab8c | 63 | dis[i] = pad.getNorm(i); |
piroro4560 | 0:424b608bab8c | 64 | rad[i] = PI - rad[i]; |
piroro4560 | 0:424b608bab8c | 65 | } |
piroro4560 | 0:424b608bab8c | 66 | X = dis[0] * cos(rad[0]); |
piroro4560 | 0:424b608bab8c | 67 | Y = dis[0] * sin(rad[0]); |
piroro4560 | 0:424b608bab8c | 68 | R1370.update(); |
piroro4560 | 0:424b608bab8c | 69 | for (int i = 0; i < 11; i++) b3[i] = b2[i] - b[i]; |
piroro4560 | 0:424b608bab8c | 70 | /** |
piroro4560 | 0:424b608bab8c | 71 | * ここまでテンプル |
piroro4560 | 0:424b608bab8c | 72 | */ |
piroro4560 | 0:424b608bab8c | 73 | start_angle = R1370.getAngle(); |
piroro4560 | 0:424b608bab8c | 74 | |
piroro4560 | 0:424b608bab8c | 75 | if ( b3[9] == 1) angle_state++; |
piroro4560 | 0:424b608bab8c | 76 | if ( b3[7] == 1) angle_state--; |
piroro4560 | 0:424b608bab8c | 77 | angle_state %= 4; |
piroro4560 | 0:424b608bab8c | 78 | switch( ( angle_state + 8 ) % 4 ){ |
piroro4560 | 0:424b608bab8c | 79 | case 1: original_angle=180; |
piroro4560 | 0:424b608bab8c | 80 | break; |
piroro4560 | 0:424b608bab8c | 81 | case 2: original_angle=-90; |
piroro4560 | 0:424b608bab8c | 82 | break; |
piroro4560 | 0:424b608bab8c | 83 | case 3: original_angle=0; |
piroro4560 | 0:424b608bab8c | 84 | break; |
piroro4560 | 0:424b608bab8c | 85 | default : original_angle=90; |
piroro4560 | 0:424b608bab8c | 86 | break; |
piroro4560 | 0:424b608bab8c | 87 | } |
piroro4560 | 0:424b608bab8c | 88 | if ( b[6] ) original_angle = start_angle; |
piroro4560 | 0:424b608bab8c | 89 | |
piroro4560 | 0:424b608bab8c | 90 | now_angle = start_angle - original_angle; |
piroro4560 | 0:424b608bab8c | 91 | |
piroro4560 | 0:424b608bab8c | 92 | if ( now_angle > 180 ) now_angle = now_angle - 360; |
piroro4560 | 0:424b608bab8c | 93 | if ( now_angle < -180 ) now_angle = now_angle + 360; |
piroro4560 | 0:424b608bab8c | 94 | |
piroro4560 | 0:424b608bab8c | 95 | pid1.setProcessValue(now_angle); |
piroro4560 | 0:424b608bab8c | 96 | /** |
piroro4560 | 0:424b608bab8c | 97 | * 定型文 |
piroro4560 | 0:424b608bab8c | 98 | */ |
piroro4560 | 0:424b608bab8c | 99 | spin_power = pid1.compute(); |
piroro4560 | 0:424b608bab8c | 100 | omni.computeXY(X,Y,-spin_power); |
piroro4560 | 0:424b608bab8c | 101 | pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||%d\n\r",X, Y, start_angle, now_angle, spin_power, angle_state); |
piroro4560 | 0:424b608bab8c | 102 | for(int i = 0; i < 4; i++)motor[i].setSpeed(omni.wheel[i]); |
piroro4560 | 0:424b608bab8c | 103 | for (int i = 0; i < 11; i++) b2[i] = b[i]; |
piroro4560 | 0:424b608bab8c | 104 | }else{ |
piroro4560 | 0:424b608bab8c | 105 | pc.printf("error\n\r"); |
piroro4560 | 0:424b608bab8c | 106 | for (int i = 0; i < 4; i++)motor[i].setSpeed(0); |
piroro4560 | 0:424b608bab8c | 107 | } |
piroro4560 | 0:424b608bab8c | 108 | } |
piroro4560 | 0:424b608bab8c | 109 | } |