PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
main.cpp@15:a9b36208dc32, 2015-10-01 (annotated)
- Committer:
- WAT34
- Date:
- Thu Oct 01 10:41:45 2015 +0000
- Revision:
- 15:a9b36208dc32
- Parent:
- 14:5722e243f843
- Child:
- 16:6b8766c77d29
tilt formula is ajusted
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 3:d2c733b52600 | 1 | #define pi 3.141593 |
WAT34 | 0:00fcc71314cf | 2 | #include "mbed.h" |
WAT34 | 0:00fcc71314cf | 3 | #include "Motor.h" |
WAT34 | 3:d2c733b52600 | 4 | #include "PID.h" |
WAT34 | 3:d2c733b52600 | 5 | #include "QEI.h" |
WAT34 | 11:427bb7a43d7a | 6 | #include "Servo.h" |
WAT34 | 4:646562d80dc2 | 7 | #define RATE 0.05 |
WAT34 | 11:427bb7a43d7a | 8 | BusOut air(p5,p6); |
WAT34 | 11:427bb7a43d7a | 9 | DigitalOut out(p7); |
WAT34 | 14:5722e243f843 | 10 | Serial conn(p9,p10); |
WAT34 | 2:74c543a0a671 | 11 | Serial pc(USBTX,USBRX); |
WAT34 | 14:5722e243f843 | 12 | Serial slave(p28,p27); |
eil4nyqn | 13:400d640bb447 | 13 | Servo L(p25); |
eil4nyqn | 13:400d640bb447 | 14 | Servo R(p26); |
eil4nyqn | 13:400d640bb447 | 15 | //PID Tp(50,40000,0,0.001); |
eil4nyqn | 13:400d640bb447 | 16 | PID Tp(4.,40000,0,0.001); |
WAT34 | 14:5722e243f843 | 17 | BusOut led(p8,p13,p14,p24); |
WAT34 | 14:5722e243f843 | 18 | Motor ot(p23,p19,p20); |
WAT34 | 2:74c543a0a671 | 19 | QEI sensort(p29,p30,NC,624); |
WAT34 | 14:5722e243f843 | 20 | Motor sup1(p21,p15,p16); |
eil4nyqn | 13:400d640bb447 | 21 | Motor sup2(p22,p17,p18); |
eil4nyqn | 13:400d640bb447 | 22 | //Moter ajust(p24,p13,p14); |
eil4nyqn | 13:400d640bb447 | 23 | BusOut ajust(p13,p14); |
eil4nyqn | 13:400d640bb447 | 24 | PwmOut ajstpwm(p24); |
eil4nyqn | 13:400d640bb447 | 25 | DigitalIn limit1(p11,PullUp); |
eil4nyqn | 13:400d640bb447 | 26 | DigitalIn limit2(p12,PullUp); |
WAT34 | 3:d2c733b52600 | 27 | Timeout ai; |
WAT34 | 0:00fcc71314cf | 28 | char read; |
WAT34 | 11:427bb7a43d7a | 29 | int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0; |
WAT34 | 3:d2c733b52600 | 30 | int i = 0; |
WAT34 | 11:427bb7a43d7a | 31 | void supply() |
WAT34 | 11:427bb7a43d7a | 32 | { |
WAT34 | 11:427bb7a43d7a | 33 | if(d%2){ |
WAT34 | 14:5722e243f843 | 34 | sup1.speed(0.9); |
WAT34 | 11:427bb7a43d7a | 35 | }else{ |
WAT34 | 14:5722e243f843 | 36 | sup2.speed(0.9); |
WAT34 | 11:427bb7a43d7a | 37 | } |
WAT34 | 14:5722e243f843 | 38 | d++; |
WAT34 | 11:427bb7a43d7a | 39 | } |
WAT34 | 11:427bb7a43d7a | 40 | void zero() |
WAT34 | 11:427bb7a43d7a | 41 | { |
WAT34 | 3:d2c733b52600 | 42 | air = 0; |
WAT34 | 3:d2c733b52600 | 43 | i = 0; |
WAT34 | 3:d2c733b52600 | 44 | out = 1; |
WAT34 | 11:427bb7a43d7a | 45 | supply(); |
WAT34 | 3:d2c733b52600 | 46 | } |
WAT34 | 11:427bb7a43d7a | 47 | void rev() |
WAT34 | 11:427bb7a43d7a | 48 | { |
WAT34 | 3:d2c733b52600 | 49 | air = 2; |
WAT34 | 3:d2c733b52600 | 50 | ai.attach(&zero,1.0); |
WAT34 | 3:d2c733b52600 | 51 | out = 0; |
WAT34 | 3:d2c733b52600 | 52 | } |
WAT34 | 2:74c543a0a671 | 53 | int main() |
WAT34 | 0:00fcc71314cf | 54 | { |
WAT34 | 4:646562d80dc2 | 55 | double tilt = 0,lo = 0,ro = 0; |
eil4nyqn | 13:400d640bb447 | 56 | int8_t ttilt = 0,tmpread = 0,tmpttilt = 0,ajst = 0; |
WAT34 | 5:4b462b9cb255 | 57 | char tro = 0,tlo = 0; |
WAT34 | 4:646562d80dc2 | 58 | Tp.setInputLimits(-45,45); |
WAT34 | 3:d2c733b52600 | 59 | Tp.setOutputLimits(-0.9,0.9); |
WAT34 | 3:d2c733b52600 | 60 | Tp.setMode(1); |
WAT34 | 4:646562d80dc2 | 61 | Tp.setBias(0.0); |
WAT34 | 0:00fcc71314cf | 62 | while(1) { |
WAT34 | 3:d2c733b52600 | 63 | if(conn.getc() == 255) { |
WAT34 | 4:646562d80dc2 | 64 | tmpread = conn.getc(); |
WAT34 | 4:646562d80dc2 | 65 | tmpttilt = conn.getc(); |
WAT34 | 5:4b462b9cb255 | 66 | tro = conn.getc(); |
WAT34 | 5:4b462b9cb255 | 67 | tlo = conn.getc(); |
eil4nyqn | 13:400d640bb447 | 68 | ajst = conn.getc(); |
WAT34 | 11:427bb7a43d7a | 69 | if(tmpread^tmpttilt^tro^tlo == conn.getc()) { |
WAT34 | 4:646562d80dc2 | 70 | ttilt = tmpttilt; |
WAT34 | 4:646562d80dc2 | 71 | read = tmpread; |
WAT34 | 5:4b462b9cb255 | 72 | ro = (tro-127)/127.0*0.9; |
WAT34 | 5:4b462b9cb255 | 73 | lo = (tlo-127)/127.0*0.9; |
WAT34 | 4:646562d80dc2 | 74 | } |
WAT34 | 0:00fcc71314cf | 75 | } |
WAT34 | 11:427bb7a43d7a | 76 | if((read>>2)%2 && i == 0) { |
WAT34 | 3:d2c733b52600 | 77 | air = 1; |
WAT34 | 3:d2c733b52600 | 78 | ai.attach(&rev,1.0); |
WAT34 | 3:d2c733b52600 | 79 | i = 1; |
WAT34 | 0:00fcc71314cf | 80 | } |
WAT34 | 11:427bb7a43d7a | 81 | /*補給制御*/ |
WAT34 | 11:427bb7a43d7a | 82 | if(limit1 == 0) { |
WAT34 | 11:427bb7a43d7a | 83 | su1 = 1; |
WAT34 | 11:427bb7a43d7a | 84 | } |
WAT34 | 11:427bb7a43d7a | 85 | if(limit2 == 0){ |
WAT34 | 11:427bb7a43d7a | 86 | su2 = 1; |
WAT34 | 11:427bb7a43d7a | 87 | } |
WAT34 | 11:427bb7a43d7a | 88 | if(limit1 == 1 && su1 == 1) { |
WAT34 | 11:427bb7a43d7a | 89 | su1 = 0; |
WAT34 | 11:427bb7a43d7a | 90 | sup1.speed(0); |
WAT34 | 11:427bb7a43d7a | 91 | } |
WAT34 | 11:427bb7a43d7a | 92 | if(limit2 == 1 && su2 == 1) { |
WAT34 | 11:427bb7a43d7a | 93 | su2 = 0; |
WAT34 | 11:427bb7a43d7a | 94 | sup2.speed(0); |
WAT34 | 11:427bb7a43d7a | 95 | } |
eil4nyqn | 13:400d640bb447 | 96 | /*輪の調整用カッコカリ*/ |
eil4nyqn | 13:400d640bb447 | 97 | if(ajst == 1){ |
eil4nyqn | 13:400d640bb447 | 98 | ajust = 1; |
eil4nyqn | 13:400d640bb447 | 99 | ajstpwm = 0.8; |
eil4nyqn | 13:400d640bb447 | 100 | }else if(ajust == 2){ |
eil4nyqn | 13:400d640bb447 | 101 | ajust = 2; |
WAT34 | 14:5722e243f843 | 102 | |
eil4nyqn | 13:400d640bb447 | 103 | ajstpwm = 0.8; |
eil4nyqn | 13:400d640bb447 | 104 | }else{ |
eil4nyqn | 13:400d640bb447 | 105 | ajust = 0; |
eil4nyqn | 13:400d640bb447 | 106 | ajstpwm = 1; |
eil4nyqn | 13:400d640bb447 | 107 | } |
WAT34 | 11:427bb7a43d7a | 108 | /*射角制御*/ |
WAT34 | 3:d2c733b52600 | 109 | Tp.setSetPoint(ttilt); |
WAT34 | 4:646562d80dc2 | 110 | tilt = double(sensort.getPulses()); |
WAT34 | 15:a9b36208dc32 | 111 | tilt = tilt*55/2684.0; |
WAT34 | 3:d2c733b52600 | 112 | Tp.setProcessValue(tilt); |
WAT34 | 11:427bb7a43d7a | 113 | /*足の出力が小さい場合はゼロとする*/ |
WAT34 | 11:427bb7a43d7a | 114 | if (abs(lo) < 0.1) { |
WAT34 | 4:646562d80dc2 | 115 | lo = 0; |
WAT34 | 4:646562d80dc2 | 116 | } |
WAT34 | 11:427bb7a43d7a | 117 | if (abs(ro) < 0.1) { |
WAT34 | 4:646562d80dc2 | 118 | ro = 0; |
WAT34 | 4:646562d80dc2 | 119 | } |
WAT34 | 11:427bb7a43d7a | 120 | /*スレーブにreadを送信*/ |
WAT34 | 11:427bb7a43d7a | 121 | slave.putc(read); |
WAT34 | 11:427bb7a43d7a | 122 | /*各モーターに出力*/ |
WAT34 | 11:427bb7a43d7a | 123 | L = (lo+1.0)/2.0; |
WAT34 | 11:427bb7a43d7a | 124 | R = (ro+1.0)/2.0;; |
WAT34 | 3:d2c733b52600 | 125 | ot.speed(Tp.compute()); |
WAT34 | 15:a9b36208dc32 | 126 | pc.printf("%d\n\r",sensort.getPulses()); |
WAT34 | 7:920cbfb28112 | 127 | //pc.printf("%f %d\n\r",Tp.compute(),read); |
eil4nyqn | 6:21f6a2216fad | 128 | //pc.printf("%d-%d\r\n",tlo,tro); |
WAT34 | 4:646562d80dc2 | 129 | wait_ms(1); |
WAT34 | 0:00fcc71314cf | 130 | } |
WAT34 | 0:00fcc71314cf | 131 | } |