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@16:6b8766c77d29, 2015-10-05 (annotated)

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