PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
main.cpp@20:e05fdab66e68, 2015-10-18 (annotated)
- Committer:
- WAT34
- Date:
- Sun Oct 18 01:06:45 2015 +0000
- Revision:
- 20:e05fdab66e68
- Parent:
- 19:f1c567cb5bb8
- Child:
- 21:5ca943882097
SUPPLY IS NOT WORK AUTOMATIC
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 | 16:6b8766c77d29 | 8 | #define shoottime 0.5 |
WAT34 | 11:427bb7a43d7a | 9 | BusOut air(p5,p6); |
WAT34 | 11:427bb7a43d7a | 10 | DigitalOut out(p7); |
WAT34 | 14:5722e243f843 | 11 | Serial conn(p9,p10); |
WAT34 | 2:74c543a0a671 | 12 | Serial pc(USBTX,USBRX); |
WAT34 | 14:5722e243f843 | 13 | Serial slave(p28,p27); |
WAT34 | 19:f1c567cb5bb8 | 14 | PwmOut Ll(LED1); |
WAT34 | 19:f1c567cb5bb8 | 15 | PwmOut Rl(LED2); |
eil4nyqn | 13:400d640bb447 | 16 | Servo L(p25); |
eil4nyqn | 13:400d640bb447 | 17 | Servo R(p26); |
WAT34 | 20:e05fdab66e68 | 18 | PID Tp(50,4000000,0,0.001); |
WAT34 | 16:6b8766c77d29 | 19 | //PID Tp(4.,40000,0,0.001); |
WAT34 | 14:5722e243f843 | 20 | BusOut led(p8,p13,p14,p24); |
WAT34 | 14:5722e243f843 | 21 | Motor ot(p23,p19,p20); |
WAT34 | 2:74c543a0a671 | 22 | QEI sensort(p29,p30,NC,624); |
WAT34 | 19:f1c567cb5bb8 | 23 | //Motor sup1(p21,p15,p16); |
WAT34 | 19:f1c567cb5bb8 | 24 | //Motor sup2(p22,p17,p18); |
WAT34 | 19:f1c567cb5bb8 | 25 | SoftPWM sup1p(p21); |
WAT34 | 19:f1c567cb5bb8 | 26 | BusOut sup1d(p15,p16); |
WAT34 | 19:f1c567cb5bb8 | 27 | SoftPWM sup2p(p22); |
WAT34 | 19:f1c567cb5bb8 | 28 | BusOut sup2d(p17,p18); |
eil4nyqn | 13:400d640bb447 | 29 | DigitalIn limit1(p11,PullUp); |
eil4nyqn | 13:400d640bb447 | 30 | DigitalIn limit2(p12,PullUp); |
WAT34 | 3:d2c733b52600 | 31 | Timeout ai; |
WAT34 | 0:00fcc71314cf | 32 | char read; |
WAT34 | 11:427bb7a43d7a | 33 | int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0; |
WAT34 | 3:d2c733b52600 | 34 | int i = 0; |
WAT34 | 19:f1c567cb5bb8 | 35 | void sup1_speed(float speed) |
WAT34 | 19:f1c567cb5bb8 | 36 | { |
WAT34 | 19:f1c567cb5bb8 | 37 | if(speed>0){ |
WAT34 | 19:f1c567cb5bb8 | 38 | sup1d = 1; |
WAT34 | 19:f1c567cb5bb8 | 39 | }else if(speed < 0){ |
WAT34 | 19:f1c567cb5bb8 | 40 | sup1d =2; |
WAT34 | 19:f1c567cb5bb8 | 41 | } |
WAT34 | 19:f1c567cb5bb8 | 42 | sup1p = fabs(speed); |
WAT34 | 19:f1c567cb5bb8 | 43 | } |
WAT34 | 19:f1c567cb5bb8 | 44 | void sup2_speed(float speed) |
WAT34 | 19:f1c567cb5bb8 | 45 | { |
WAT34 | 19:f1c567cb5bb8 | 46 | if(speed>0){ |
WAT34 | 19:f1c567cb5bb8 | 47 | sup2d = 1; |
WAT34 | 19:f1c567cb5bb8 | 48 | }else if(speed < 0){ |
WAT34 | 19:f1c567cb5bb8 | 49 | sup2d =2; |
WAT34 | 19:f1c567cb5bb8 | 50 | } |
WAT34 | 19:f1c567cb5bb8 | 51 | sup2p = fabs(speed); |
WAT34 | 19:f1c567cb5bb8 | 52 | } |
WAT34 | 11:427bb7a43d7a | 53 | void supply() |
WAT34 | 11:427bb7a43d7a | 54 | { |
WAT34 | 11:427bb7a43d7a | 55 | if(d%2){ |
WAT34 | 19:f1c567cb5bb8 | 56 | sup1_speed(0.9); |
WAT34 | 11:427bb7a43d7a | 57 | }else{ |
WAT34 | 19:f1c567cb5bb8 | 58 | sup2_speed(0.9); |
WAT34 | 11:427bb7a43d7a | 59 | } |
WAT34 | 14:5722e243f843 | 60 | d++; |
WAT34 | 11:427bb7a43d7a | 61 | } |
WAT34 | 11:427bb7a43d7a | 62 | void zero() |
WAT34 | 11:427bb7a43d7a | 63 | { |
WAT34 | 3:d2c733b52600 | 64 | air = 0; |
WAT34 | 3:d2c733b52600 | 65 | i = 0; |
WAT34 | 3:d2c733b52600 | 66 | out = 1; |
WAT34 | 20:e05fdab66e68 | 67 | //supply(); |
WAT34 | 3:d2c733b52600 | 68 | } |
WAT34 | 11:427bb7a43d7a | 69 | void rev() |
WAT34 | 11:427bb7a43d7a | 70 | { |
WAT34 | 3:d2c733b52600 | 71 | air = 2; |
WAT34 | 16:6b8766c77d29 | 72 | ai.attach(&zero,shoottime); |
WAT34 | 3:d2c733b52600 | 73 | out = 0; |
WAT34 | 3:d2c733b52600 | 74 | } |
WAT34 | 2:74c543a0a671 | 75 | int main() |
WAT34 | 0:00fcc71314cf | 76 | { |
WAT34 | 16:6b8766c77d29 | 77 | led = 1; |
WAT34 | 4:646562d80dc2 | 78 | double tilt = 0,lo = 0,ro = 0; |
eil4nyqn | 13:400d640bb447 | 79 | int8_t ttilt = 0,tmpread = 0,tmpttilt = 0,ajst = 0; |
WAT34 | 5:4b462b9cb255 | 80 | char tro = 0,tlo = 0; |
WAT34 | 4:646562d80dc2 | 81 | Tp.setInputLimits(-45,45); |
WAT34 | 3:d2c733b52600 | 82 | Tp.setOutputLimits(-0.9,0.9); |
WAT34 | 3:d2c733b52600 | 83 | Tp.setMode(1); |
WAT34 | 4:646562d80dc2 | 84 | Tp.setBias(0.0); |
WAT34 | 16:6b8766c77d29 | 85 | L = 0.5; |
WAT34 | 16:6b8766c77d29 | 86 | R = 0.5; |
WAT34 | 16:6b8766c77d29 | 87 | wait(3); |
WAT34 | 16:6b8766c77d29 | 88 | led= 15; |
WAT34 | 19:f1c567cb5bb8 | 89 | conn.baud(38400); |
WAT34 | 0:00fcc71314cf | 90 | while(1) { |
WAT34 | 20:e05fdab66e68 | 91 | //ps3コントローラーシリアル受信 |
WAT34 | 3:d2c733b52600 | 92 | if(conn.getc() == 255) { |
WAT34 | 19:f1c567cb5bb8 | 93 | read = conn.getc(); |
WAT34 | 20:e05fdab66e68 | 94 | ttilt = conn.getc()-60; |
WAT34 | 19:f1c567cb5bb8 | 95 | ro = (conn.getc()-127)/127.0*0.9; |
WAT34 | 19:f1c567cb5bb8 | 96 | lo = (conn.getc()-127)/127.0*0.9; |
WAT34 | 0:00fcc71314cf | 97 | } |
WAT34 | 20:e05fdab66e68 | 98 | //nara シリアル受信 |
WAT34 | 20:e05fdab66e68 | 99 | if(slave.avaiable(){ |
WAT34 | 20:e05fdab66e68 | 100 | narasup = slave.getc(); |
WAT34 | 20:e05fdab66e68 | 101 | } |
WAT34 | 20:e05fdab66e68 | 102 | if(narasup == 1){ |
WAT34 | 20:e05fdab66e68 | 103 | sup1_speed(0.9); |
WAT34 | 20:e05fdab66e68 | 104 | sup2_speed(0); |
WAT34 | 20:e05fdab66e68 | 105 | }else if (narasup == 2){ |
WAT34 | 20:e05fdab66e68 | 106 | sup1_speed(-0.9); |
WAT34 | 20:e05fdab66e68 | 107 | sup2_speed(0); |
WAT34 | 20:e05fdab66e68 | 108 | }else if (narasup == 4){ |
WAT34 | 20:e05fdab66e68 | 109 | sup2_speed(0.9); |
WAT34 | 20:e05fdab66e68 | 110 | sup1_speed(0); |
WAT34 | 20:e05fdab66e68 | 111 | }else if (narasup ==8){ |
WAT34 | 20:e05fdab66e68 | 112 | sup2_speed(-0.9); |
WAT34 | 20:e05fdab66e68 | 113 | sup1_speed(0); |
WAT34 | 20:e05fdab66e68 | 114 | }else { |
WAT34 | 20:e05fdab66e68 | 115 | sup1_speed(0); |
WAT34 | 20:e05fdab66e68 | 116 | sup2_speed(0); |
WAT34 | 20:e05fdab66e68 | 117 | } |
WAT34 | 20:e05fdab66e68 | 118 | |
WAT34 | 19:f1c567cb5bb8 | 119 | if(read>4){ |
WAT34 | 19:f1c567cb5bb8 | 120 | read = 0; |
WAT34 | 19:f1c567cb5bb8 | 121 | } |
WAT34 | 19:f1c567cb5bb8 | 122 | led = read; |
WAT34 | 19:f1c567cb5bb8 | 123 | if((read>>1)%2 && i == 0) { |
WAT34 | 3:d2c733b52600 | 124 | air = 1; |
WAT34 | 16:6b8766c77d29 | 125 | ai.attach(&rev,shoottime); |
WAT34 | 3:d2c733b52600 | 126 | i = 1; |
WAT34 | 0:00fcc71314cf | 127 | } |
WAT34 | 11:427bb7a43d7a | 128 | /*補給制御*/ |
WAT34 | 20:e05fdab66e68 | 129 | /*if(limit1 == 0) { |
WAT34 | 11:427bb7a43d7a | 130 | su1 = 1; |
WAT34 | 11:427bb7a43d7a | 131 | } |
WAT34 | 11:427bb7a43d7a | 132 | if(limit2 == 0){ |
WAT34 | 11:427bb7a43d7a | 133 | su2 = 1; |
WAT34 | 11:427bb7a43d7a | 134 | } |
WAT34 | 11:427bb7a43d7a | 135 | if(limit1 == 1 && su1 == 1) { |
WAT34 | 11:427bb7a43d7a | 136 | su1 = 0; |
WAT34 | 19:f1c567cb5bb8 | 137 | sup1_speed(0); |
WAT34 | 11:427bb7a43d7a | 138 | } |
WAT34 | 11:427bb7a43d7a | 139 | if(limit2 == 1 && su2 == 1) { |
WAT34 | 11:427bb7a43d7a | 140 | su2 = 0; |
WAT34 | 19:f1c567cb5bb8 | 141 | sup2_speed(0); |
WAT34 | 20:e05fdab66e68 | 142 | }*/ |
WAT34 | 11:427bb7a43d7a | 143 | /*射角制御*/ |
WAT34 | 20:e05fdab66e68 | 144 | Tp.setSetPoint(ttilt/2.0); |
WAT34 | 4:646562d80dc2 | 145 | tilt = double(sensort.getPulses()); |
WAT34 | 19:f1c567cb5bb8 | 146 | tilt = tilt*49.0/1745.0; |
WAT34 | 3:d2c733b52600 | 147 | Tp.setProcessValue(tilt); |
WAT34 | 11:427bb7a43d7a | 148 | /*足の出力が小さい場合はゼロとする*/ |
WAT34 | 11:427bb7a43d7a | 149 | if (abs(lo) < 0.1) { |
WAT34 | 4:646562d80dc2 | 150 | lo = 0; |
WAT34 | 4:646562d80dc2 | 151 | } |
WAT34 | 11:427bb7a43d7a | 152 | if (abs(ro) < 0.1) { |
WAT34 | 4:646562d80dc2 | 153 | ro = 0; |
WAT34 | 4:646562d80dc2 | 154 | } |
WAT34 | 11:427bb7a43d7a | 155 | /*スレーブにreadを送信*/ |
WAT34 | 11:427bb7a43d7a | 156 | slave.putc(read); |
WAT34 | 11:427bb7a43d7a | 157 | /*各モーターに出力*/ |
WAT34 | 16:6b8766c77d29 | 158 | L = (ro+1.0)/2.0; |
WAT34 | 19:f1c567cb5bb8 | 159 | Ll = (ro+1.0)/2.0; |
WAT34 | 16:6b8766c77d29 | 160 | R = 1.0-(lo+1.0)/2.0; |
WAT34 | 19:f1c567cb5bb8 | 161 | Rl = 1.0-(lo+1.0)/2.0; |
WAT34 | 3:d2c733b52600 | 162 | ot.speed(Tp.compute()); |
WAT34 | 16:6b8766c77d29 | 163 | //pc.printf("%f\n\r",tilt); |
WAT34 | 16:6b8766c77d29 | 164 | //pc.printf("%d\n\r",sensort.getPulses()); |
WAT34 | 16:6b8766c77d29 | 165 | //pc.printf("%f \n\r",Tp.compute()); |
WAT34 | 0:00fcc71314cf | 166 | } |
WAT34 | 0:00fcc71314cf | 167 | } |