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@4:646562d80dc2, 2015-09-03 (annotated)

Committer:
WAT34
Date:
Thu Sep 03 02:37:51 2015 +0000
Revision:
4:646562d80dc2
Parent:
3:d2c733b52600
Child:
5:4b462b9cb255
??PID????

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 Lp(4.7,6,0,RATE);
WAT34 4:646562d80dc2 14 PID Rp(4.7,6,0,RATE);
WAT34 4:646562d80dc2 15 PID Tp(50,40000,0,0.001);
WAT34 3:d2c733b52600 16 BusOut led(LED1,LED2,LED3,LED4);
WAT34 4:646562d80dc2 17 Motor ot(p23,p19,p20);
WAT34 2:74c543a0a671 18 QEI sensort(p29,p30,NC,624);
WAT34 3:d2c733b52600 19 QEI ls(p27,p28,NC,624);
WAT34 3:d2c733b52600 20 QEI rs(p25,p26,NC,624);
WAT34 3:d2c733b52600 21 Timeout ai;
WAT34 3:d2c733b52600 22 Ticker getsp;
WAT34 0:00fcc71314cf 23 char read;
WAT34 3:d2c733b52600 24 int Rs = 0,Ls = 0;
WAT34 3:d2c733b52600 25 int i = 0;
WAT34 3:d2c733b52600 26 void getspeed()
WAT34 3:d2c733b52600 27 {
WAT34 4:646562d80dc2 28 Rs =rs.getPulses()*2;
WAT34 4:646562d80dc2 29 Ls =ls.getPulses()*2;
WAT34 3:d2c733b52600 30 rs.reset();
WAT34 3:d2c733b52600 31 ls.reset();
WAT34 3:d2c733b52600 32 }
WAT34 3:d2c733b52600 33 void zero(){
WAT34 3:d2c733b52600 34 air = 0;
WAT34 3:d2c733b52600 35 i = 0;
WAT34 3:d2c733b52600 36 out = 1;
WAT34 3:d2c733b52600 37 }
WAT34 3:d2c733b52600 38 void rev(){
WAT34 3:d2c733b52600 39 air = 2;
WAT34 3:d2c733b52600 40 ai.attach(&zero,1.0);
WAT34 3:d2c733b52600 41 out = 0;
WAT34 3:d2c733b52600 42 }
WAT34 2:74c543a0a671 43 int main()
WAT34 0:00fcc71314cf 44 {
WAT34 4:646562d80dc2 45 double tilt = 0,lo = 0,ro = 0;
WAT34 4:646562d80dc2 46 int8_t ttilt = 0,tmpread = 0,tmpttilt = 0;
WAT34 3:d2c733b52600 47 Lp.setInputLimits(-3000,3000);
WAT34 3:d2c733b52600 48 Rp.setInputLimits(-3000,3000);
WAT34 4:646562d80dc2 49 Tp.setInputLimits(-45,45);
WAT34 3:d2c733b52600 50 Lp.setOutputLimits(-0.9,0.9);
WAT34 3:d2c733b52600 51 Rp.setOutputLimits(-0.9,0.9);
WAT34 3:d2c733b52600 52 Tp.setOutputLimits(-0.9,0.9);
WAT34 3:d2c733b52600 53 Lp.setMode(1);
WAT34 3:d2c733b52600 54 Rp.setMode(1);
WAT34 3:d2c733b52600 55 Tp.setMode(1);
WAT34 4:646562d80dc2 56 Lp.setBias(0.0);
WAT34 4:646562d80dc2 57 Rp.setBias(0.0);
WAT34 4:646562d80dc2 58 Tp.setBias(0.0);
WAT34 4:646562d80dc2 59 getsp.attach(&getspeed,0.05);
WAT34 0:00fcc71314cf 60 while(1) {
WAT34 3:d2c733b52600 61 if(conn.getc() == 255) {
WAT34 4:646562d80dc2 62 tmpread = conn.getc();
WAT34 4:646562d80dc2 63 tmpttilt = conn.getc();
WAT34 4:646562d80dc2 64 if(tmpread^tmpttilt == conn.getc()){
WAT34 4:646562d80dc2 65 ttilt = tmpttilt;
WAT34 4:646562d80dc2 66 read = tmpread;
WAT34 4:646562d80dc2 67 }
WAT34 0:00fcc71314cf 68 }
WAT34 3:d2c733b52600 69 if((read>>2)%2 && i == 0){
WAT34 3:d2c733b52600 70 air = 1;
WAT34 3:d2c733b52600 71 ai.attach(&rev,1.0);
WAT34 3:d2c733b52600 72 i = 1;
WAT34 3:d2c733b52600 73 led = 1;
WAT34 0:00fcc71314cf 74 }
WAT34 3:d2c733b52600 75 if((read>>3)%2){
WAT34 4:646562d80dc2 76 Lp.setSetPoint(500);
WAT34 4:646562d80dc2 77 Rp.setSetPoint(500);
WAT34 3:d2c733b52600 78 }else
WAT34 3:d2c733b52600 79 if((read>>4)%2){
WAT34 4:646562d80dc2 80 Lp.setSetPoint(-500);
WAT34 4:646562d80dc2 81 Rp.setSetPoint(-500);
WAT34 3:d2c733b52600 82 led = 2;
WAT34 3:d2c733b52600 83 }else
WAT34 3:d2c733b52600 84 if((read>>5)%2){
WAT34 4:646562d80dc2 85 Lp.setSetPoint(-500);
WAT34 4:646562d80dc2 86 Rp.setSetPoint(500);
WAT34 3:d2c733b52600 87 led = 4;
WAT34 3:d2c733b52600 88 }else
WAT34 3:d2c733b52600 89 if((read>>6)%2){
WAT34 4:646562d80dc2 90 Lp.setSetPoint(500);
WAT34 4:646562d80dc2 91 Rp.setSetPoint(-500);
WAT34 3:d2c733b52600 92 led = 8;
WAT34 3:d2c733b52600 93 }else{
WAT34 3:d2c733b52600 94 Lp.setSetPoint(0);
WAT34 3:d2c733b52600 95 Rp.setSetPoint(0);
WAT34 3:d2c733b52600 96 led = 0;
WAT34 0:00fcc71314cf 97 }
WAT34 3:d2c733b52600 98 Tp.setSetPoint(ttilt);
WAT34 4:646562d80dc2 99 tilt = double(sensort.getPulses());
WAT34 4:646562d80dc2 100 tilt = tilt*61/5128.0;
WAT34 3:d2c733b52600 101 Lp.setProcessValue(Ls);
WAT34 3:d2c733b52600 102 Rp.setProcessValue(Rs);
WAT34 3:d2c733b52600 103 Tp.setProcessValue(tilt);
WAT34 4:646562d80dc2 104 lo = Lp.compute();
WAT34 4:646562d80dc2 105 ro = Rp.compute();
WAT34 4:646562d80dc2 106 if (abs(lo) < 0.1){
WAT34 4:646562d80dc2 107 lo = 0;
WAT34 4:646562d80dc2 108 }
WAT34 4:646562d80dc2 109 if (abs(ro) < 0.1){
WAT34 4:646562d80dc2 110 ro = 0;
WAT34 4:646562d80dc2 111 }
WAT34 4:646562d80dc2 112 L.speed(lo);
WAT34 4:646562d80dc2 113 R.speed(ro);
WAT34 3:d2c733b52600 114 ot.speed(Tp.compute());
WAT34 4:646562d80dc2 115 pc.printf("%d\n\r",ttilt);
WAT34 4:646562d80dc2 116 wait_ms(1);
WAT34 0:00fcc71314cf 117 }
WAT34 0:00fcc71314cf 118 }