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@17:c55a8b8a3eb2, 2015-10-07 (annotated)

Committer:
WAT34
Date:
Wed Oct 07 07:47:49 2015 +0000
Revision:
17:c55a8b8a3eb2
Parent:
16:6b8766c77d29
Child:
19:f1c567cb5bb8
ps3 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 17:c55a8b8a3eb2 16 PID Tp(50,40000,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();
WAT34 11:427bb7a43d7a 71 if(tmpread^tmpttilt^tro^tlo == conn.getc()) {
WAT34 4:646562d80dc2 72 ttilt = tmpttilt;
WAT34 4:646562d80dc2 73 read = tmpread;
WAT34 16:6b8766c77d29 74 ro = (tro-117)/127.0*0.9;
WAT34 5:4b462b9cb255 75 lo = (tlo-127)/127.0*0.9;
WAT34 4:646562d80dc2 76 }
WAT34 0:00fcc71314cf 77 }
WAT34 11:427bb7a43d7a 78 if((read>>2)%2 && i == 0) {
WAT34 3:d2c733b52600 79 air = 1;
WAT34 16:6b8766c77d29 80 ai.attach(&rev,shoottime);
WAT34 3:d2c733b52600 81 i = 1;
WAT34 0:00fcc71314cf 82 }
WAT34 11:427bb7a43d7a 83 /*補給制御*/
WAT34 11:427bb7a43d7a 84 if(limit1 == 0) {
WAT34 11:427bb7a43d7a 85 su1 = 1;
WAT34 11:427bb7a43d7a 86 }
WAT34 11:427bb7a43d7a 87 if(limit2 == 0){
WAT34 11:427bb7a43d7a 88 su2 = 1;
WAT34 11:427bb7a43d7a 89 }
WAT34 11:427bb7a43d7a 90 if(limit1 == 1 && su1 == 1) {
WAT34 11:427bb7a43d7a 91 su1 = 0;
WAT34 11:427bb7a43d7a 92 sup1.speed(0);
WAT34 11:427bb7a43d7a 93 }
WAT34 11:427bb7a43d7a 94 if(limit2 == 1 && su2 == 1) {
WAT34 11:427bb7a43d7a 95 su2 = 0;
WAT34 11:427bb7a43d7a 96 sup2.speed(0);
WAT34 11:427bb7a43d7a 97 }
WAT34 11:427bb7a43d7a 98 /*射角制御*/
WAT34 3:d2c733b52600 99 Tp.setSetPoint(ttilt);
WAT34 4:646562d80dc2 100 tilt = double(sensort.getPulses());
WAT34 15:a9b36208dc32 101 tilt = tilt*55/2684.0;
WAT34 3:d2c733b52600 102 Tp.setProcessValue(tilt);
WAT34 11:427bb7a43d7a 103 /*足の出力が小さい場合はゼロとする*/
WAT34 11:427bb7a43d7a 104 if (abs(lo) < 0.1) {
WAT34 4:646562d80dc2 105 lo = 0;
WAT34 4:646562d80dc2 106 }
WAT34 11:427bb7a43d7a 107 if (abs(ro) < 0.1) {
WAT34 4:646562d80dc2 108 ro = 0;
WAT34 4:646562d80dc2 109 }
WAT34 11:427bb7a43d7a 110 /*スレーブにreadを送信*/
WAT34 11:427bb7a43d7a 111 slave.putc(read);
WAT34 11:427bb7a43d7a 112 /*各モーターに出力*/
WAT34 16:6b8766c77d29 113 L = (ro+1.0)/2.0;
WAT34 16:6b8766c77d29 114 R = 1.0-(lo+1.0)/2.0;
WAT34 3:d2c733b52600 115 ot.speed(Tp.compute());
WAT34 16:6b8766c77d29 116 //pc.printf("%f\n\r",tilt);
WAT34 16:6b8766c77d29 117 //pc.printf("%d\n\r",sensort.getPulses());
WAT34 16:6b8766c77d29 118 //pc.printf("%f \n\r",Tp.compute());
WAT34 17:c55a8b8a3eb2 119 pc.printf("%d-%d\r\n",tlo,tro);
WAT34 17:c55a8b8a3eb2 120 wait_ms(20);
WAT34 0:00fcc71314cf 121 }
WAT34 0:00fcc71314cf 122 }