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

動くといいな

Dependencies:   Motor_NIT_Nagaoka_College PID Servo QEI SoftPWM mbed HBridge

main.cpp@18:7703c9baf008, 2015-10-09 (annotated)

Committer:
WAT34
Date:
Fri Oct 09 07:25:39 2015 +0000
Revision:
18:7703c9baf008
Parent:
16:6b8766c77d29 
new library

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