mbedを用いた制御学生の制御 / Mbed 2 deprecated NHK2015_new

PS3 version

Dependencies:   Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn

Fork of NHK2015 by mbedを用いた制御学生の制御

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?

UserRevisionLine numberNew 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 }