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@3:d2c733b52600, 2015-09-02 (annotated)

Committer:
WAT34
Date:
Wed Sep 02 00:19:34 2015 +0000
Revision:
3:d2c733b52600
Parent:
2:74c543a0a671
Child:
4:646562d80dc2
??????????

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 3:d2c733b52600 6 #define RATE 0.01
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 3:d2c733b52600 11 Motor L(p22,p15,p16);
WAT34 3:d2c733b52600 12 Motor R(p23,p17,p18);
WAT34 3:d2c733b52600 13 PID Lp(1.0,0,0,RATE);
WAT34 3:d2c733b52600 14 PID Rp(1.0,0,0,RATE);
WAT34 3:d2c733b52600 15 PID Tp(1.0,0,0,RATE);
WAT34 3:d2c733b52600 16 BusOut led(LED1,LED2,LED3,LED4);
WAT34 3:d2c733b52600 17 Motor ot(p21,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 3:d2c733b52600 28 Rs =rs.getPulses();
WAT34 3:d2c733b52600 29 Ls =ls.getPulses();
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 3:d2c733b52600 45 double tilt = 0;
WAT34 3:d2c733b52600 46 int8_t ttilt = 0;
WAT34 3:d2c733b52600 47 Lp.setInputLimits(-3000,3000);
WAT34 3:d2c733b52600 48 Rp.setInputLimits(-3000,3000);
WAT34 3:d2c733b52600 49 Tp.setInputLimits(-10000,10000);
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 3:d2c733b52600 56 Lp.setBias(0);
WAT34 3:d2c733b52600 57 Rp.setBias(0);
WAT34 3:d2c733b52600 58 Tp.setBias(0);
WAT34 3:d2c733b52600 59 getsp.attach(&getspeed,0.1);
WAT34 0:00fcc71314cf 60 while(1) {
WAT34 3:d2c733b52600 61 if(conn.getc() == 255) {
WAT34 0:00fcc71314cf 62 read = conn.getc();
WAT34 3:d2c733b52600 63 ttilt = conn.getc();
WAT34 0:00fcc71314cf 64 }
WAT34 3:d2c733b52600 65 if((read>>2)%2 && i == 0){
WAT34 3:d2c733b52600 66 air = 1;
WAT34 3:d2c733b52600 67 ai.attach(&rev,1.0);
WAT34 3:d2c733b52600 68 i = 1;
WAT34 3:d2c733b52600 69 led = 1;
WAT34 0:00fcc71314cf 70 }
WAT34 3:d2c733b52600 71 if((read>>3)%2){
WAT34 3:d2c733b52600 72 Lp.setSetPoint(100);
WAT34 3:d2c733b52600 73 Rp.setSetPoint(100);
WAT34 3:d2c733b52600 74 }else
WAT34 3:d2c733b52600 75 if((read>>4)%2){
WAT34 3:d2c733b52600 76 Lp.setSetPoint(-100);
WAT34 3:d2c733b52600 77 Rp.setSetPoint(-100);
WAT34 3:d2c733b52600 78 led = 2;
WAT34 3:d2c733b52600 79 }else
WAT34 3:d2c733b52600 80 if((read>>5)%2){
WAT34 3:d2c733b52600 81 Lp.setSetPoint(-100);
WAT34 3:d2c733b52600 82 Rp.setSetPoint(100);
WAT34 3:d2c733b52600 83 led = 4;
WAT34 3:d2c733b52600 84 }else
WAT34 3:d2c733b52600 85 if((read>>6)%2){
WAT34 3:d2c733b52600 86 Lp.setSetPoint(100);
WAT34 3:d2c733b52600 87 Rp.setSetPoint(-100);
WAT34 3:d2c733b52600 88 led = 8;
WAT34 3:d2c733b52600 89 }else{
WAT34 3:d2c733b52600 90 Lp.setSetPoint(0);
WAT34 3:d2c733b52600 91 Rp.setSetPoint(0);
WAT34 3:d2c733b52600 92 led = 0;
WAT34 0:00fcc71314cf 93 }
WAT34 3:d2c733b52600 94 Tp.setSetPoint(ttilt);
WAT34 3:d2c733b52600 95 tilt = sensort.getPulses();
WAT34 3:d2c733b52600 96 //tilt = tilt*24.0/500.0*pi/360.0;
WAT34 3:d2c733b52600 97 conn.putc(int8_t(tilt));
WAT34 3:d2c733b52600 98 Lp.setProcessValue(Ls);
WAT34 3:d2c733b52600 99 Rp.setProcessValue(Rs);
WAT34 3:d2c733b52600 100 Tp.setProcessValue(tilt);
WAT34 3:d2c733b52600 101 L.speed(Lp.compute());
WAT34 3:d2c733b52600 102 R.speed(Rp.compute());
WAT34 3:d2c733b52600 103 ot.speed(Tp.compute());
WAT34 3:d2c733b52600 104 pc.printf("%f\n\r",Lp.compute());
WAT34 0:00fcc71314cf 105 }
WAT34 0:00fcc71314cf 106 }