PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
main.cpp@23:bfe8c3791f1f, 2015-10-26 (annotated)
- Committer:
- WAT34
- Date:
- Mon Oct 26 07:10:02 2015 +0000
- Revision:
- 23:bfe8c3791f1f
- Parent:
- 22:a834fd13fcfd
- Child:
- 24:7e93db7d1c32
brushless final version
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 | 22:a834fd13fcfd | 7 | #include "DebounceIn.h" |
WAT34 | 4:646562d80dc2 | 8 | #define RATE 0.05 |
WAT34 | 16:6b8766c77d29 | 9 | #define shoottime 0.5 |
WAT34 | 23:bfe8c3791f1f | 10 | #define DEADTIME 500 |
WAT34 | 11:427bb7a43d7a | 11 | BusOut air(p5,p6); |
WAT34 | 11:427bb7a43d7a | 12 | DigitalOut out(p7); |
WAT34 | 14:5722e243f843 | 13 | Serial conn(p9,p10); |
WAT34 | 2:74c543a0a671 | 14 | Serial pc(USBTX,USBRX); |
WAT34 | 14:5722e243f843 | 15 | Serial slave(p28,p27); |
WAT34 | 19:f1c567cb5bb8 | 16 | PwmOut Ll(LED1); |
WAT34 | 19:f1c567cb5bb8 | 17 | PwmOut Rl(LED2); |
WAT34 | 23:bfe8c3791f1f | 18 | BusOut mled(LED3,LED4); |
eil4nyqn | 13:400d640bb447 | 19 | Servo L(p25); |
eil4nyqn | 13:400d640bb447 | 20 | Servo R(p26); |
WAT34 | 20:e05fdab66e68 | 21 | PID Tp(50,4000000,0,0.001); |
WAT34 | 16:6b8766c77d29 | 22 | //PID Tp(4.,40000,0,0.001); |
WAT34 | 14:5722e243f843 | 23 | BusOut led(p8,p13,p14,p24); |
WAT34 | 22:a834fd13fcfd | 24 | Motor ot(p23,p20,p19); |
WAT34 | 2:74c543a0a671 | 25 | QEI sensort(p29,p30,NC,624); |
WAT34 | 19:f1c567cb5bb8 | 26 | //Motor sup1(p21,p15,p16); |
WAT34 | 19:f1c567cb5bb8 | 27 | //Motor sup2(p22,p17,p18); |
WAT34 | 19:f1c567cb5bb8 | 28 | SoftPWM sup1p(p21); |
WAT34 | 19:f1c567cb5bb8 | 29 | BusOut sup1d(p15,p16); |
WAT34 | 19:f1c567cb5bb8 | 30 | SoftPWM sup2p(p22); |
WAT34 | 22:a834fd13fcfd | 31 | BusOut sup2d(p18,p17); |
WAT34 | 22:a834fd13fcfd | 32 | DebounceIn limit1(p11); |
WAT34 | 22:a834fd13fcfd | 33 | DebounceIn limit2(p12); |
WAT34 | 3:d2c733b52600 | 34 | Timeout ai; |
WAT34 | 23:bfe8c3791f1f | 35 | Timer timer1; |
WAT34 | 23:bfe8c3791f1f | 36 | Timer timer2; |
WAT34 | 0:00fcc71314cf | 37 | char read; |
WAT34 | 11:427bb7a43d7a | 38 | int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0; |
WAT34 | 3:d2c733b52600 | 39 | int i = 0; |
WAT34 | 22:a834fd13fcfd | 40 | int old_limit1 = 0,old_limit2 = 0,new_limit1 = 0,new_limit2 = 0,count1 = 0,count2 = 0,target_count1 = 0,target_count2 = 0,supply_seq1 = 0,supply_seq2 = 0; |
WAT34 | 19:f1c567cb5bb8 | 41 | void sup1_speed(float speed) |
WAT34 | 19:f1c567cb5bb8 | 42 | { |
WAT34 | 21:5ca943882097 | 43 | if(speed>0) { |
WAT34 | 19:f1c567cb5bb8 | 44 | sup1d = 1; |
WAT34 | 21:5ca943882097 | 45 | } else if(speed < 0) { |
WAT34 | 19:f1c567cb5bb8 | 46 | sup1d =2; |
WAT34 | 19:f1c567cb5bb8 | 47 | } |
WAT34 | 19:f1c567cb5bb8 | 48 | sup1p = fabs(speed); |
WAT34 | 19:f1c567cb5bb8 | 49 | } |
WAT34 | 19:f1c567cb5bb8 | 50 | void sup2_speed(float speed) |
WAT34 | 19:f1c567cb5bb8 | 51 | { |
WAT34 | 21:5ca943882097 | 52 | if(speed>0) { |
WAT34 | 19:f1c567cb5bb8 | 53 | sup2d = 1; |
WAT34 | 21:5ca943882097 | 54 | } else if(speed < 0) { |
WAT34 | 19:f1c567cb5bb8 | 55 | sup2d =2; |
WAT34 | 19:f1c567cb5bb8 | 56 | } |
WAT34 | 19:f1c567cb5bb8 | 57 | sup2p = fabs(speed); |
WAT34 | 19:f1c567cb5bb8 | 58 | } |
WAT34 | 21:5ca943882097 | 59 | void sup1_brake() |
WAT34 | 21:5ca943882097 | 60 | { |
WAT34 | 21:5ca943882097 | 61 | sup1d = 0; |
WAT34 | 21:5ca943882097 | 62 | sup1p = 1; |
WAT34 | 21:5ca943882097 | 63 | } |
WAT34 | 21:5ca943882097 | 64 | void sup2_brake() |
WAT34 | 21:5ca943882097 | 65 | { |
WAT34 | 21:5ca943882097 | 66 | sup2d = 0; |
WAT34 | 21:5ca943882097 | 67 | sup2p = 1; |
WAT34 | 21:5ca943882097 | 68 | } |
WAT34 | 23:bfe8c3791f1f | 69 | void supply_1() |
WAT34 | 23:bfe8c3791f1f | 70 | { |
WAT34 | 23:bfe8c3791f1f | 71 | sup1_speed(0.9); |
WAT34 | 23:bfe8c3791f1f | 72 | target_count1 +=3; |
WAT34 | 23:bfe8c3791f1f | 73 | supply_seq1 = 1; |
WAT34 | 23:bfe8c3791f1f | 74 | } |
WAT34 | 23:bfe8c3791f1f | 75 | void supply_2() |
WAT34 | 23:bfe8c3791f1f | 76 | { |
WAT34 | 23:bfe8c3791f1f | 77 | sup2_speed(0.9); |
WAT34 | 23:bfe8c3791f1f | 78 | target_count2 +=3; |
WAT34 | 23:bfe8c3791f1f | 79 | supply_seq2 = 1; |
WAT34 | 23:bfe8c3791f1f | 80 | } |
WAT34 | 11:427bb7a43d7a | 81 | void supply() |
WAT34 | 11:427bb7a43d7a | 82 | { |
WAT34 | 23:bfe8c3791f1f | 83 | if (supply_seq1 ==0 && supply_seq2 == 0) { |
WAT34 | 23:bfe8c3791f1f | 84 | if(d%2) { |
WAT34 | 23:bfe8c3791f1f | 85 | supply_1(); |
WAT34 | 23:bfe8c3791f1f | 86 | } else { |
WAT34 | 23:bfe8c3791f1f | 87 | supply_2(); |
WAT34 | 23:bfe8c3791f1f | 88 | } |
WAT34 | 23:bfe8c3791f1f | 89 | d++; |
WAT34 | 11:427bb7a43d7a | 90 | } |
WAT34 | 11:427bb7a43d7a | 91 | } |
WAT34 | 23:bfe8c3791f1f | 92 | |
WAT34 | 23:bfe8c3791f1f | 93 | void count_check() |
WAT34 | 23:bfe8c3791f1f | 94 | { |
WAT34 | 23:bfe8c3791f1f | 95 | if((target_count1 == count1)&&(supply_seq1 == 1)) { |
WAT34 | 22:a834fd13fcfd | 96 | sup1_speed(-0.9); |
WAT34 | 22:a834fd13fcfd | 97 | target_count1 += 2; |
WAT34 | 22:a834fd13fcfd | 98 | supply_seq1 = 2; |
WAT34 | 23:bfe8c3791f1f | 99 | } else if((target_count1 == count1) && (supply_seq1 == 2)) { |
WAT34 | 23:bfe8c3791f1f | 100 | sup1_brake(); |
WAT34 | 22:a834fd13fcfd | 101 | supply_seq1 = 0; |
WAT34 | 23:bfe8c3791f1f | 102 | mled = 0; |
WAT34 | 22:a834fd13fcfd | 103 | } |
WAT34 | 23:bfe8c3791f1f | 104 | if((target_count2 == count2)&&(supply_seq2 == 1)) { |
WAT34 | 22:a834fd13fcfd | 105 | sup2_speed(-0.9); |
WAT34 | 22:a834fd13fcfd | 106 | target_count2 += 2; |
WAT34 | 22:a834fd13fcfd | 107 | supply_seq2 = 2; |
WAT34 | 23:bfe8c3791f1f | 108 | } else if((target_count2 == count2) && (supply_seq2 == 2)) { |
WAT34 | 23:bfe8c3791f1f | 109 | sup2_brake(); |
WAT34 | 22:a834fd13fcfd | 110 | supply_seq2 = 0; |
WAT34 | 23:bfe8c3791f1f | 111 | mled = 0; |
WAT34 | 22:a834fd13fcfd | 112 | } |
WAT34 | 22:a834fd13fcfd | 113 | } |
WAT34 | 22:a834fd13fcfd | 114 | void limit1_count() |
WAT34 | 22:a834fd13fcfd | 115 | { |
WAT34 | 22:a834fd13fcfd | 116 | new_limit1 = limit1; |
WAT34 | 22:a834fd13fcfd | 117 | if ((new_limit1==0) && (old_limit1==1)) count1++; |
WAT34 | 22:a834fd13fcfd | 118 | old_limit1 = new_limit1; |
WAT34 | 22:a834fd13fcfd | 119 | } |
WAT34 | 22:a834fd13fcfd | 120 | void limit2_count() |
WAT34 | 22:a834fd13fcfd | 121 | { |
WAT34 | 22:a834fd13fcfd | 122 | new_limit2 = limit2; |
WAT34 | 22:a834fd13fcfd | 123 | if ((new_limit2==0) && (old_limit2==1)) count2++; |
WAT34 | 22:a834fd13fcfd | 124 | old_limit2 = new_limit2; |
WAT34 | 22:a834fd13fcfd | 125 | } |
WAT34 | 11:427bb7a43d7a | 126 | void zero() |
WAT34 | 11:427bb7a43d7a | 127 | { |
WAT34 | 3:d2c733b52600 | 128 | air = 0; |
WAT34 | 3:d2c733b52600 | 129 | i = 0; |
WAT34 | 3:d2c733b52600 | 130 | out = 1; |
WAT34 | 3:d2c733b52600 | 131 | } |
WAT34 | 11:427bb7a43d7a | 132 | void rev() |
WAT34 | 11:427bb7a43d7a | 133 | { |
WAT34 | 3:d2c733b52600 | 134 | air = 2; |
WAT34 | 16:6b8766c77d29 | 135 | ai.attach(&zero,shoottime); |
WAT34 | 3:d2c733b52600 | 136 | out = 0; |
WAT34 | 3:d2c733b52600 | 137 | } |
WAT34 | 2:74c543a0a671 | 138 | int main() |
WAT34 | 0:00fcc71314cf | 139 | { |
WAT34 | 22:a834fd13fcfd | 140 | limit1.mode(PullUp); |
WAT34 | 22:a834fd13fcfd | 141 | limit2.mode(PullUp); |
WAT34 | 16:6b8766c77d29 | 142 | led = 1; |
WAT34 | 23:bfe8c3791f1f | 143 | int dead1 = 0,dead2 = 0; |
WAT34 | 4:646562d80dc2 | 144 | double tilt = 0,lo = 0,ro = 0; |
WAT34 | 23:bfe8c3791f1f | 145 | int8_t ttilt = 60,tmpread = 0,tmpttilt = 0,ajst = 0; |
WAT34 | 23:bfe8c3791f1f | 146 | char tro = 0,tlo = 0,narasup = 60; |
WAT34 | 4:646562d80dc2 | 147 | Tp.setInputLimits(-45,45); |
WAT34 | 3:d2c733b52600 | 148 | Tp.setOutputLimits(-0.9,0.9); |
WAT34 | 3:d2c733b52600 | 149 | Tp.setMode(1); |
WAT34 | 4:646562d80dc2 | 150 | Tp.setBias(0.0); |
WAT34 | 23:bfe8c3791f1f | 151 | Tp.setProcessValue(sensort.getPulses()*49.0/1745.0); |
WAT34 | 23:bfe8c3791f1f | 152 | Tp.setSetPoint(0); |
WAT34 | 23:bfe8c3791f1f | 153 | ot.speed(0); |
WAT34 | 16:6b8766c77d29 | 154 | L = 0.5; |
WAT34 | 16:6b8766c77d29 | 155 | R = 0.5; |
WAT34 | 23:bfe8c3791f1f | 156 | wait(5); |
WAT34 | 16:6b8766c77d29 | 157 | led= 15; |
WAT34 | 19:f1c567cb5bb8 | 158 | conn.baud(38400); |
WAT34 | 0:00fcc71314cf | 159 | while(1) { |
WAT34 | 20:e05fdab66e68 | 160 | //ps3コントローラーシリアル受信 |
WAT34 | 3:d2c733b52600 | 161 | if(conn.getc() == 255) { |
WAT34 | 19:f1c567cb5bb8 | 162 | read = conn.getc(); |
WAT34 | 23:bfe8c3791f1f | 163 | ttilt = conn.getc(); |
WAT34 | 23:bfe8c3791f1f | 164 | tro =conn.getc(); |
WAT34 | 23:bfe8c3791f1f | 165 | tlo = conn.getc(); |
WAT34 | 23:bfe8c3791f1f | 166 | if (tro == 255){ |
WAT34 | 23:bfe8c3791f1f | 167 | ro =ro; |
WAT34 | 23:bfe8c3791f1f | 168 | }else{ |
WAT34 | 23:bfe8c3791f1f | 169 | ro = (tro-127)/127.0*0.9; |
WAT34 | 23:bfe8c3791f1f | 170 | } |
WAT34 | 23:bfe8c3791f1f | 171 | if(tlo ==255){ |
WAT34 | 23:bfe8c3791f1f | 172 | lo = lo; |
WAT34 | 23:bfe8c3791f1f | 173 | }else { |
WAT34 | 23:bfe8c3791f1f | 174 | lo = (tlo-127)/127.0*0.9; |
WAT34 | 23:bfe8c3791f1f | 175 | } |
WAT34 | 0:00fcc71314cf | 176 | } |
WAT34 | 20:e05fdab66e68 | 177 | //nara シリアル受信 |
WAT34 | 21:5ca943882097 | 178 | if(slave.readable()) { |
WAT34 | 22:a834fd13fcfd | 179 | narasup = slave.getc(); |
WAT34 | 23:bfe8c3791f1f | 180 | if((narasup-60) == 1 && supply_seq1 == 0) { |
WAT34 | 23:bfe8c3791f1f | 181 | mled = 1; |
WAT34 | 23:bfe8c3791f1f | 182 | supply_1(); |
WAT34 | 23:bfe8c3791f1f | 183 | } else if((narasup-60) == 2 && supply_seq2 == 0) { |
WAT34 | 23:bfe8c3791f1f | 184 | supply_2(); |
WAT34 | 23:bfe8c3791f1f | 185 | mled =2; |
WAT34 | 23:bfe8c3791f1f | 186 | } |
WAT34 | 20:e05fdab66e68 | 187 | } |
WAT34 | 23:bfe8c3791f1f | 188 | if (read > 4){ //通信不良対策 |
WAT34 | 22:a834fd13fcfd | 189 | read = 0; |
WAT34 | 22:a834fd13fcfd | 190 | } |
WAT34 | 22:a834fd13fcfd | 191 | led = read; |
WAT34 | 23:bfe8c3791f1f | 192 | if(read==2 && i == 0) { |
WAT34 | 22:a834fd13fcfd | 193 | air = 1; |
WAT34 | 22:a834fd13fcfd | 194 | ai.attach(&rev,shoottime); |
WAT34 | 3:d2c733b52600 | 195 | i = 1; |
WAT34 | 22:a834fd13fcfd | 196 | supply(); |
WAT34 | 11:427bb7a43d7a | 197 | } |
WAT34 | 22:a834fd13fcfd | 198 | limit1_count(); |
WAT34 | 22:a834fd13fcfd | 199 | limit2_count(); |
WAT34 | 22:a834fd13fcfd | 200 | count_check(); |
WAT34 | 11:427bb7a43d7a | 201 | /*射角制御*/ |
WAT34 | 23:bfe8c3791f1f | 202 | Tp.setSetPoint((ttilt-60)/2.0); |
WAT34 | 4:646562d80dc2 | 203 | tilt = double(sensort.getPulses()); |
WAT34 | 19:f1c567cb5bb8 | 204 | tilt = tilt*49.0/1745.0; |
WAT34 | 3:d2c733b52600 | 205 | Tp.setProcessValue(tilt); |
WAT34 | 23:bfe8c3791f1f | 206 | ro *= abs(ro)*0.5; |
WAT34 | 23:bfe8c3791f1f | 207 | lo *= abs(lo)*0.5; |
WAT34 | 11:427bb7a43d7a | 208 | /*足の出力が小さい場合はゼロとする*/ |
WAT34 | 11:427bb7a43d7a | 209 | if (abs(lo) < 0.1) { |
WAT34 | 22:a834fd13fcfd | 210 | lo = 0; |
WAT34 | 22:a834fd13fcfd | 211 | } |
WAT34 | 22:a834fd13fcfd | 212 | if (abs(ro) < 0.1) { |
WAT34 | 22:a834fd13fcfd | 213 | ro = 0; |
WAT34 | 22:a834fd13fcfd | 214 | } |
WAT34 | 23:bfe8c3791f1f | 215 | if(timer1.read_ms() > DEADTIME) { |
WAT34 | 23:bfe8c3791f1f | 216 | dead1 = 0; |
WAT34 | 23:bfe8c3791f1f | 217 | timer1.stop(); |
WAT34 | 23:bfe8c3791f1f | 218 | timer1.reset(); |
WAT34 | 23:bfe8c3791f1f | 219 | } |
WAT34 | 23:bfe8c3791f1f | 220 | if(timer2.read_ms() > DEADTIME) { |
WAT34 | 23:bfe8c3791f1f | 221 | dead2 = 0; |
WAT34 | 23:bfe8c3791f1f | 222 | timer2.stop(); |
WAT34 | 23:bfe8c3791f1f | 223 | timer2.reset(); |
WAT34 | 23:bfe8c3791f1f | 224 | } |
WAT34 | 23:bfe8c3791f1f | 225 | if ((L >0.5 && (lo+1.0)/2.0<=0.5) || (L <0.5 && (lo+1.0)/2.0>=0.5) || dead1 ==1) { |
WAT34 | 23:bfe8c3791f1f | 226 | L = 0.5; |
WAT34 | 23:bfe8c3791f1f | 227 | Ll = 0.5; |
WAT34 | 23:bfe8c3791f1f | 228 | dead1 =1; |
WAT34 | 23:bfe8c3791f1f | 229 | timer1.start(); |
WAT34 | 23:bfe8c3791f1f | 230 | } else { |
WAT34 | 23:bfe8c3791f1f | 231 | L = (lo+1.0)/2.0; |
WAT34 | 23:bfe8c3791f1f | 232 | Ll = (lo+1.0)/2.0; |
WAT34 | 23:bfe8c3791f1f | 233 | } |
WAT34 | 23:bfe8c3791f1f | 234 | if ( (R >0.5 && (ro+1.0)/2.0<=0.5) || (R <0.5 && (ro+1.0)/2.0>=0.5) || dead2 ==1) { |
WAT34 | 23:bfe8c3791f1f | 235 | R = 0.5; |
WAT34 | 23:bfe8c3791f1f | 236 | Rl = 0.5; |
WAT34 | 23:bfe8c3791f1f | 237 | dead2 = 1; |
WAT34 | 23:bfe8c3791f1f | 238 | timer2.start(); |
WAT34 | 23:bfe8c3791f1f | 239 | } else { |
WAT34 | 23:bfe8c3791f1f | 240 | R = (ro+1.0)/2.0; |
WAT34 | 23:bfe8c3791f1f | 241 | Rl = (ro+1.0)/2.0; |
WAT34 | 23:bfe8c3791f1f | 242 | } |
WAT34 | 22:a834fd13fcfd | 243 | /*スレーブにreadを送信*/ |
WAT34 | 22:a834fd13fcfd | 244 | slave.putc(read); |
WAT34 | 22:a834fd13fcfd | 245 | /*各モーターに出力*/ |
WAT34 | 23:bfe8c3791f1f | 246 | pc.printf("%d\n\r",narasup); |
WAT34 | 22:a834fd13fcfd | 247 | ot.speed(Tp.compute()); |
WAT34 | 22:a834fd13fcfd | 248 | //pc.printf("%d\n\r",sensort.getPulses()); |
WAT34 | 22:a834fd13fcfd | 249 | //pc.printf("%f \n\r",Tp.compute()); |
WAT34 | 0:00fcc71314cf | 250 | } |
WAT34 | 0:00fcc71314cf | 251 | } |