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@24:7e93db7d1c32, 2015-10-30 (annotated)

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