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@22:a834fd13fcfd, 2015-10-19 (annotated)

Committer:
WAT34
Date:
Mon Oct 19 08:23:12 2015 +0000
Revision:
22:a834fd13fcfd
Parent:
21:5ca943882097
Child:
23:bfe8c3791f1f
?????Ver2;

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