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

動くといいな

Dependencies:   Motor_NIT_Nagaoka_College PID Servo QEI SoftPWM mbed HBridge

main.cpp@15:a9b36208dc32, 2015-10-01 (annotated)

Committer:
WAT34
Date:
Thu Oct 01 10:41:45 2015 +0000
Revision:
15:a9b36208dc32
Parent:
14:5722e243f843
Child:
16:6b8766c77d29
tilt formula is ajusted

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 4:646562d80dc2 7 #define RATE 0.05
WAT34 11:427bb7a43d7a 8 BusOut air(p5,p6);
WAT34 11:427bb7a43d7a 9 DigitalOut out(p7);
WAT34 14:5722e243f843 10 Serial conn(p9,p10);
WAT34 2:74c543a0a671 11 Serial pc(USBTX,USBRX);
WAT34 14:5722e243f843 12 Serial slave(p28,p27);
eil4nyqn 13:400d640bb447 13 Servo L(p25);
eil4nyqn 13:400d640bb447 14 Servo R(p26);
eil4nyqn 13:400d640bb447 15 //PID Tp(50,40000,0,0.001);
eil4nyqn 13:400d640bb447 16 PID Tp(4.,40000,0,0.001);
WAT34 14:5722e243f843 17 BusOut led(p8,p13,p14,p24);
WAT34 14:5722e243f843 18 Motor ot(p23,p19,p20);
WAT34 2:74c543a0a671 19 QEI sensort(p29,p30,NC,624);
WAT34 14:5722e243f843 20 Motor sup1(p21,p15,p16);
eil4nyqn 13:400d640bb447 21 Motor sup2(p22,p17,p18);
eil4nyqn 13:400d640bb447 22 //Moter ajust(p24,p13,p14);
eil4nyqn 13:400d640bb447 23 BusOut ajust(p13,p14);
eil4nyqn 13:400d640bb447 24 PwmOut ajstpwm(p24);
eil4nyqn 13:400d640bb447 25 DigitalIn limit1(p11,PullUp);
eil4nyqn 13:400d640bb447 26 DigitalIn limit2(p12,PullUp);
WAT34 3:d2c733b52600 27 Timeout ai;
WAT34 0:00fcc71314cf 28 char read;
WAT34 11:427bb7a43d7a 29 int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
WAT34 3:d2c733b52600 30 int i = 0;
WAT34 11:427bb7a43d7a 31 void supply()
WAT34 11:427bb7a43d7a 32 {
WAT34 11:427bb7a43d7a 33 if(d%2){
WAT34 14:5722e243f843 34 sup1.speed(0.9);
WAT34 11:427bb7a43d7a 35 }else{
WAT34 14:5722e243f843 36 sup2.speed(0.9);
WAT34 11:427bb7a43d7a 37 }
WAT34 14:5722e243f843 38 d++;
WAT34 11:427bb7a43d7a 39 }
WAT34 11:427bb7a43d7a 40 void zero()
WAT34 11:427bb7a43d7a 41 {
WAT34 3:d2c733b52600 42 air = 0;
WAT34 3:d2c733b52600 43 i = 0;
WAT34 3:d2c733b52600 44 out = 1;
WAT34 11:427bb7a43d7a 45 supply();
WAT34 3:d2c733b52600 46 }
WAT34 11:427bb7a43d7a 47 void rev()
WAT34 11:427bb7a43d7a 48 {
WAT34 3:d2c733b52600 49 air = 2;
WAT34 3:d2c733b52600 50 ai.attach(&zero,1.0);
WAT34 3:d2c733b52600 51 out = 0;
WAT34 3:d2c733b52600 52 }
WAT34 2:74c543a0a671 53 int main()
WAT34 0:00fcc71314cf 54 {
WAT34 4:646562d80dc2 55 double tilt = 0,lo = 0,ro = 0;
eil4nyqn 13:400d640bb447 56 int8_t ttilt = 0,tmpread = 0,tmpttilt = 0,ajst = 0;
WAT34 5:4b462b9cb255 57 char tro = 0,tlo = 0;
WAT34 4:646562d80dc2 58 Tp.setInputLimits(-45,45);
WAT34 3:d2c733b52600 59 Tp.setOutputLimits(-0.9,0.9);
WAT34 3:d2c733b52600 60 Tp.setMode(1);
WAT34 4:646562d80dc2 61 Tp.setBias(0.0);
WAT34 0:00fcc71314cf 62 while(1) {
WAT34 3:d2c733b52600 63 if(conn.getc() == 255) {
WAT34 4:646562d80dc2 64 tmpread = conn.getc();
WAT34 4:646562d80dc2 65 tmpttilt = conn.getc();
WAT34 5:4b462b9cb255 66 tro = conn.getc();
WAT34 5:4b462b9cb255 67 tlo = conn.getc();
eil4nyqn 13:400d640bb447 68 ajst = conn.getc();
WAT34 11:427bb7a43d7a 69 if(tmpread^tmpttilt^tro^tlo == conn.getc()) {
WAT34 4:646562d80dc2 70 ttilt = tmpttilt;
WAT34 4:646562d80dc2 71 read = tmpread;
WAT34 5:4b462b9cb255 72 ro = (tro-127)/127.0*0.9;
WAT34 5:4b462b9cb255 73 lo = (tlo-127)/127.0*0.9;
WAT34 4:646562d80dc2 74 }
WAT34 0:00fcc71314cf 75 }
WAT34 11:427bb7a43d7a 76 if((read>>2)%2 && i == 0) {
WAT34 3:d2c733b52600 77 air = 1;
WAT34 3:d2c733b52600 78 ai.attach(&rev,1.0);
WAT34 3:d2c733b52600 79 i = 1;
WAT34 0:00fcc71314cf 80 }
WAT34 11:427bb7a43d7a 81 /*補給制御*/
WAT34 11:427bb7a43d7a 82 if(limit1 == 0) {
WAT34 11:427bb7a43d7a 83 su1 = 1;
WAT34 11:427bb7a43d7a 84 }
WAT34 11:427bb7a43d7a 85 if(limit2 == 0){
WAT34 11:427bb7a43d7a 86 su2 = 1;
WAT34 11:427bb7a43d7a 87 }
WAT34 11:427bb7a43d7a 88 if(limit1 == 1 && su1 == 1) {
WAT34 11:427bb7a43d7a 89 su1 = 0;
WAT34 11:427bb7a43d7a 90 sup1.speed(0);
WAT34 11:427bb7a43d7a 91 }
WAT34 11:427bb7a43d7a 92 if(limit2 == 1 && su2 == 1) {
WAT34 11:427bb7a43d7a 93 su2 = 0;
WAT34 11:427bb7a43d7a 94 sup2.speed(0);
WAT34 11:427bb7a43d7a 95 }
eil4nyqn 13:400d640bb447 96 /*輪の調整用ｶｯｺｶﾘ*/
eil4nyqn 13:400d640bb447 97 if(ajst == 1){
eil4nyqn 13:400d640bb447 98 ajust = 1;
eil4nyqn 13:400d640bb447 99 ajstpwm = 0.8;
eil4nyqn 13:400d640bb447 100 }else if(ajust == 2){
eil4nyqn 13:400d640bb447 101 ajust = 2;
WAT34 14:5722e243f843 102
eil4nyqn 13:400d640bb447 103 ajstpwm = 0.8;
eil4nyqn 13:400d640bb447 104 }else{
eil4nyqn 13:400d640bb447 105 ajust = 0;
eil4nyqn 13:400d640bb447 106 ajstpwm = 1;
eil4nyqn 13:400d640bb447 107 }
WAT34 11:427bb7a43d7a 108 /*射角制御*/
WAT34 3:d2c733b52600 109 Tp.setSetPoint(ttilt);
WAT34 4:646562d80dc2 110 tilt = double(sensort.getPulses());
WAT34 15:a9b36208dc32 111 tilt = tilt*55/2684.0;
WAT34 3:d2c733b52600 112 Tp.setProcessValue(tilt);
WAT34 11:427bb7a43d7a 113 /*足の出力が小さい場合はゼロとする*/
WAT34 11:427bb7a43d7a 114 if (abs(lo) < 0.1) {
WAT34 4:646562d80dc2 115 lo = 0;
WAT34 4:646562d80dc2 116 }
WAT34 11:427bb7a43d7a 117 if (abs(ro) < 0.1) {
WAT34 4:646562d80dc2 118 ro = 0;
WAT34 4:646562d80dc2 119 }
WAT34 11:427bb7a43d7a 120 /*スレーブにreadを送信*/
WAT34 11:427bb7a43d7a 121 slave.putc(read);
WAT34 11:427bb7a43d7a 122 /*各モーターに出力*/
WAT34 11:427bb7a43d7a 123 L = (lo+1.0)/2.0;
WAT34 11:427bb7a43d7a 124 R = (ro+1.0)/2.0;;
WAT34 3:d2c733b52600 125 ot.speed(Tp.compute());
WAT34 15:a9b36208dc32 126 pc.printf("%d\n\r",sensort.getPulses());
WAT34 7:920cbfb28112 127 //pc.printf("%f %d\n\r",Tp.compute(),read);
eil4nyqn 6:21f6a2216fad 128 //pc.printf("%d-%d\r\n",tlo,tro);
WAT34 4:646562d80dc2 129 wait_ms(1);
WAT34 0:00fcc71314cf 130 }
WAT34 0:00fcc71314cf 131 }