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

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