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

動くといいな

Dependencies:   Motor_NIT_Nagaoka_College PID Servo QEI SoftPWM mbed HBridge

main.cpp@5:4b462b9cb255, 2015-09-04 (annotated)

Committer:
WAT34
Date:
Fri Sep 04 01:13:02 2015 +0000
Revision:
5:4b462b9cb255
Parent:
4:646562d80dc2
Child:
6:21f6a2216fad
???;

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 4:646562d80dc2 6 #define RATE 0.05
WAT34 3:d2c733b52600 7 BusOut air(p13,p14);
WAT34 3:d2c733b52600 8 DigitalOut out(p12);
WAT34 3:d2c733b52600 9 Serial conn(p9,p10);
WAT34 2:74c543a0a671 10 Serial pc(USBTX,USBRX);
WAT34 4:646562d80dc2 11 Motor L(p21,p15,p16);
WAT34 4:646562d80dc2 12 Motor R(p22,p17,p18);
WAT34 4:646562d80dc2 13 PID Tp(50,40000,0,0.001);
WAT34 3:d2c733b52600 14 BusOut led(LED1,LED2,LED3,LED4);
WAT34 4:646562d80dc2 15 Motor ot(p23,p19,p20);
WAT34 2:74c543a0a671 16 QEI sensort(p29,p30,NC,624);
WAT34 3:d2c733b52600 17 Timeout ai;
WAT34 0:00fcc71314cf 18 char read;
WAT34 3:d2c733b52600 19 int Rs = 0,Ls = 0;
WAT34 3:d2c733b52600 20 int i = 0;
WAT34 3:d2c733b52600 21 void zero(){
WAT34 3:d2c733b52600 22 air = 0;
WAT34 3:d2c733b52600 23 i = 0;
WAT34 3:d2c733b52600 24 out = 1;
WAT34 3:d2c733b52600 25 }
WAT34 3:d2c733b52600 26 void rev(){
WAT34 3:d2c733b52600 27 air = 2;
WAT34 3:d2c733b52600 28 ai.attach(&zero,1.0);
WAT34 3:d2c733b52600 29 out = 0;
WAT34 3:d2c733b52600 30 }
WAT34 2:74c543a0a671 31 int main()
WAT34 0:00fcc71314cf 32 {
WAT34 4:646562d80dc2 33 double tilt = 0,lo = 0,ro = 0;
WAT34 4:646562d80dc2 34 int8_t ttilt = 0,tmpread = 0,tmpttilt = 0;
WAT34 5:4b462b9cb255 35 char tro = 0,tlo = 0;
WAT34 4:646562d80dc2 36 Tp.setInputLimits(-45,45);
WAT34 3:d2c733b52600 37 Tp.setOutputLimits(-0.9,0.9);
WAT34 3:d2c733b52600 38 Tp.setMode(1);
WAT34 4:646562d80dc2 39 Tp.setBias(0.0);
WAT34 0:00fcc71314cf 40 while(1) {
WAT34 3:d2c733b52600 41 if(conn.getc() == 255) {
WAT34 4:646562d80dc2 42 tmpread = conn.getc();
WAT34 4:646562d80dc2 43 tmpttilt = conn.getc();
WAT34 5:4b462b9cb255 44 tro = conn.getc();
WAT34 5:4b462b9cb255 45 tlo = conn.getc();
WAT34 5:4b462b9cb255 46 if(tmpread^tmpttilt^tro^tlo == conn.getc()){
WAT34 4:646562d80dc2 47 ttilt = tmpttilt;
WAT34 4:646562d80dc2 48 read = tmpread;
WAT34 5:4b462b9cb255 49 ro = (tro-127)/127.0*0.9;
WAT34 5:4b462b9cb255 50 lo = (tlo-127)/127.0*0.9;
WAT34 4:646562d80dc2 51 }
WAT34 0:00fcc71314cf 52 }
WAT34 3:d2c733b52600 53 if((read>>2)%2 && i == 0){
WAT34 3:d2c733b52600 54 air = 1;
WAT34 3:d2c733b52600 55 ai.attach(&rev,1.0);
WAT34 3:d2c733b52600 56 i = 1;
WAT34 0:00fcc71314cf 57 }
WAT34 3:d2c733b52600 58 Tp.setSetPoint(ttilt);
WAT34 4:646562d80dc2 59 tilt = double(sensort.getPulses());
WAT34 4:646562d80dc2 60 tilt = tilt*61/5128.0;
WAT34 3:d2c733b52600 61 Tp.setProcessValue(tilt);
WAT34 4:646562d80dc2 62 if (abs(lo) < 0.1){
WAT34 4:646562d80dc2 63 lo = 0;
WAT34 4:646562d80dc2 64 }
WAT34 4:646562d80dc2 65 if (abs(ro) < 0.1){
WAT34 4:646562d80dc2 66 ro = 0;
WAT34 4:646562d80dc2 67 }
WAT34 4:646562d80dc2 68 L.speed(lo);
WAT34 4:646562d80dc2 69 R.speed(ro);
WAT34 3:d2c733b52600 70 ot.speed(Tp.compute());
WAT34 5:4b462b9cb255 71 pc.printf("%d %d\n\r",Ls,read);
WAT34 4:646562d80dc2 72 wait_ms(1);
WAT34 0:00fcc71314cf 73 }
WAT34 0:00fcc71314cf 74 }