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

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Motor_NIT_Nagaoka_College PID Servo QEI SoftPWM mbed HBridge

main.cpp@9:98ef1eee7ace, 2015-09-15 (annotated)

Committer:
WAT34
Date:
Tue Sep 15 10:58:01 2015 +0000
Revision:
9:98ef1eee7ace
Parent:
8:9d8999740604 
Now yanagi pakuri systems are equipped.

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 8:9d8999740604 6 #include "Servo.h"
WAT34 4:646562d80dc2 7 #define RATE 0.05
WAT34 8:9d8999740604 8 BusOut air(PC_10,PC_11);
WAT34 8:9d8999740604 9 DigitalOut out(PC_12);
WAT34 8:9d8999740604 10 Serial conn(PA_15,PB_7);
WAT34 2:74c543a0a671 11 Serial pc(USBTX,USBRX);
WAT34 9:98ef1eee7ace 12 Serial nara(PA_2,PA_3);
WAT34 4:646562d80dc2 13 PID Tp(50,40000,0,0.001);
WAT34 3:d2c733b52600 14 BusOut led(LED1,LED2,LED3,LED4);
WAT34 8:9d8999740604 15 Motor ot(PB_13,PB_3,PA_10);
WAT34 9:98ef1eee7ace 16 Motor pak(PB_14,PB_4,PA_11);
WAT34 9:98ef1eee7ace 17 Motor sup1(PB_15,PB_10,PB_4);
WAT34 9:98ef1eee7ace 18 Motor sup2(PA_11,PA_6,PA_7);
WAT34 8:9d8999740604 19 QEI sensort(PC_3,PC_0,NC,624);
WAT34 8:9d8999740604 20 Servo L(PC_9);
WAT34 8:9d8999740604 21 Servo R(PC_8);
WAT34 9:98ef1eee7ace 22 BusIn limits(PB_8,PB_9);
WAT34 3:d2c733b52600 23 Timeout ai;
WAT34 9:98ef1eee7ace 24 Timeout yanagi;
WAT34 9:98ef1eee7ace 25 char read ,yayaya = 0;
WAT34 3:d2c733b52600 26 int Rs = 0,Ls = 0;
WAT34 3:d2c733b52600 27 int i = 0;
WAT34 3:d2c733b52600 28 void zero(){
WAT34 3:d2c733b52600 29 air = 0;
WAT34 3:d2c733b52600 30 i = 0;
WAT34 3:d2c733b52600 31 out = 1;
WAT34 3:d2c733b52600 32 }
WAT34 3:d2c733b52600 33 void rev(){
WAT34 3:d2c733b52600 34 air = 2;
WAT34 3:d2c733b52600 35 ai.attach(&zero,1.0);
WAT34 3:d2c733b52600 36 out = 0;
WAT34 3:d2c733b52600 37 }
WAT34 9:98ef1eee7ace 38 void pakri(){
WAT34 9:98ef1eee7ace 39 yayaya = 2;
WAT34 9:98ef1eee7ace 40 }
WAT34 2:74c543a0a671 41 int main()
WAT34 0:00fcc71314cf 42 {
WAT34 8:9d8999740604 43 ot.setfrequency(60000);
WAT34 4:646562d80dc2 44 double tilt = 0,lo = 0,ro = 0;
WAT34 4:646562d80dc2 45 int8_t ttilt = 0,tmpread = 0,tmpttilt = 0;
WAT34 5:4b462b9cb255 46 char tro = 0,tlo = 0;
WAT34 4:646562d80dc2 47 Tp.setInputLimits(-45,45);
WAT34 3:d2c733b52600 48 Tp.setOutputLimits(-0.9,0.9);
WAT34 3:d2c733b52600 49 Tp.setMode(1);
WAT34 4:646562d80dc2 50 Tp.setBias(0.0);
WAT34 0:00fcc71314cf 51 while(1) {
WAT34 3:d2c733b52600 52 if(conn.getc() == 255) {
WAT34 4:646562d80dc2 53 tmpread = conn.getc();
WAT34 4:646562d80dc2 54 tmpttilt = conn.getc();
WAT34 5:4b462b9cb255 55 tro = conn.getc();
WAT34 5:4b462b9cb255 56 tlo = conn.getc();
WAT34 5:4b462b9cb255 57 if(tmpread^tmpttilt^tro^tlo == conn.getc()){
WAT34 4:646562d80dc2 58 ttilt = tmpttilt;
WAT34 4:646562d80dc2 59 read = tmpread;
WAT34 5:4b462b9cb255 60 ro = (tro-127)/127.0*0.9;
WAT34 5:4b462b9cb255 61 lo = (tlo-127)/127.0*0.9;
WAT34 4:646562d80dc2 62 }
WAT34 0:00fcc71314cf 63 }
WAT34 9:98ef1eee7ace 64 if(read%2){ //yanagi shoot.
WAT34 9:98ef1eee7ace 65 yanagi.attach(&pakri,3.0);
WAT34 9:98ef1eee7ace 66 yayaya = 1;
WAT34 9:98ef1eee7ace 67 }
WAT34 3:d2c733b52600 68 if((read>>2)%2 && i == 0){
WAT34 3:d2c733b52600 69 air = 1;
WAT34 3:d2c733b52600 70 ai.attach(&rev,1.0);
WAT34 3:d2c733b52600 71 i = 1;
WAT34 0:00fcc71314cf 72 }
WAT34 9:98ef1eee7ace 73 /* yanagi controll */
WAT34 9:98ef1eee7ace 74 if (yayaya == 1 && limits != 2) {
WAT34 9:98ef1eee7ace 75 pak.speed(1);
WAT34 9:98ef1eee7ace 76 }else if (yayaya == 2 && limits !=1) {
WAT34 9:98ef1eee7ace 77 pak.speed(-1);
WAT34 9:98ef1eee7ace 78 }else {
WAT34 9:98ef1eee7ace 79 pak.speed(0);
WAT34 9:98ef1eee7ace 80 }
WAT34 9:98ef1eee7ace 81 /* 射角調整 */
WAT34 3:d2c733b52600 82 Tp.setSetPoint(ttilt);
WAT34 4:646562d80dc2 83 tilt = double(sensort.getPulses());
WAT34 4:646562d80dc2 84 tilt = tilt*61/5128.0;
WAT34 3:d2c733b52600 85 Tp.setProcessValue(tilt);
WAT34 4:646562d80dc2 86 if (abs(lo) < 0.1){
WAT34 4:646562d80dc2 87 lo = 0;
WAT34 4:646562d80dc2 88 }
WAT34 4:646562d80dc2 89 if (abs(ro) < 0.1){
WAT34 4:646562d80dc2 90 ro = 0;
WAT34 4:646562d80dc2 91 }
WAT34 8:9d8999740604 92 L.write((lo+1.0)/2.0);
WAT34 8:9d8999740604 93 R.write((ro+1.0)/2.0);
WAT34 3:d2c733b52600 94 ot.speed(Tp.compute());
WAT34 7:920cbfb28112 95 //pc.printf("%f %d\n\r",Tp.compute(),read);
eil4nyqn 6:21f6a2216fad 96 //pc.printf("%d-%d\r\n",tlo,tro);
WAT34 4:646562d80dc2 97 wait_ms(1);
WAT34 0:00fcc71314cf 98 }
WAT34 0:00fcc71314cf 99 }