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@10:fefff533c442, 2015-09-16 (annotated)

Committer:
WAT34
Date:
Wed Sep 16 08:33:58 2015 +0000
Revision:
10:fefff533c442
Parent:
8:9d8999740604 
????

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 10:fefff533c442 8 DigitalOut led(LED1);
WAT34 8:9d8999740604 9 BusOut air(PC_10,PC_11);
WAT34 8:9d8999740604 10 DigitalOut out(PC_12);
WAT34 10:fefff533c442 11 Serial conn(NC,PA_12);
WAT34 10:fefff533c442 12 Serial pc(SERIAL_TX, SERIAL_RX);
WAT34 4:646562d80dc2 13 PID Tp(50,40000,0,0.001);
WAT34 8:9d8999740604 14 Motor ot(PB_13,PB_3,PA_10);
WAT34 8:9d8999740604 15 QEI sensort(PC_3,PC_0,NC,624);
WAT34 8:9d8999740604 16 Servo L(PC_9);
WAT34 8:9d8999740604 17 Servo R(PC_8);
WAT34 3:d2c733b52600 18 Timeout ai;
WAT34 0:00fcc71314cf 19 char read;
WAT34 3:d2c733b52600 20 int Rs = 0,Ls = 0;
WAT34 3:d2c733b52600 21 int i = 0;
WAT34 3:d2c733b52600 22 void zero(){
WAT34 3:d2c733b52600 23 air = 0;
WAT34 3:d2c733b52600 24 i = 0;
WAT34 3:d2c733b52600 25 out = 1;
WAT34 3:d2c733b52600 26 }
WAT34 3:d2c733b52600 27 void rev(){
WAT34 3:d2c733b52600 28 air = 2;
WAT34 3:d2c733b52600 29 ai.attach(&zero,1.0);
WAT34 3:d2c733b52600 30 out = 0;
WAT34 3:d2c733b52600 31 }
WAT34 2:74c543a0a671 32 int main()
WAT34 0:00fcc71314cf 33 {
WAT34 10:fefff533c442 34 pc.printf("connected\n\r");
WAT34 10:fefff533c442 35 //conn.baud(115200);
WAT34 10:fefff533c442 36 //ot.setfrequency(60000);
WAT34 4:646562d80dc2 37 double tilt = 0,lo = 0,ro = 0;
WAT34 4:646562d80dc2 38 int8_t ttilt = 0,tmpread = 0,tmpttilt = 0;
WAT34 5:4b462b9cb255 39 char tro = 0,tlo = 0;
WAT34 4:646562d80dc2 40 Tp.setInputLimits(-45,45);
WAT34 3:d2c733b52600 41 Tp.setOutputLimits(-0.9,0.9);
WAT34 3:d2c733b52600 42 Tp.setMode(1);
WAT34 10:fefff533c442 43 led=1;
WAT34 4:646562d80dc2 44 Tp.setBias(0.0);
WAT34 0:00fcc71314cf 45 while(1) {
WAT34 10:fefff533c442 46 if(conn.getc() == 0xFF) {
WAT34 10:fefff533c442 47 led = !led;
WAT34 4:646562d80dc2 48 tmpread = conn.getc();
WAT34 4:646562d80dc2 49 tmpttilt = conn.getc();
WAT34 5:4b462b9cb255 50 tro = conn.getc();
WAT34 5:4b462b9cb255 51 tlo = conn.getc();
WAT34 5:4b462b9cb255 52 if(tmpread^tmpttilt^tro^tlo == conn.getc()){
WAT34 4:646562d80dc2 53 ttilt = tmpttilt;
WAT34 4:646562d80dc2 54 read = tmpread;
WAT34 10:fefff533c442 55 ro = tro;
WAT34 10:fefff533c442 56 lo = tlo;
WAT34 4:646562d80dc2 57 }
WAT34 0:00fcc71314cf 58 }
WAT34 3:d2c733b52600 59 if((read>>2)%2 && i == 0){
WAT34 3:d2c733b52600 60 air = 1;
WAT34 3:d2c733b52600 61 ai.attach(&rev,1.0);
WAT34 3:d2c733b52600 62 i = 1;
WAT34 0:00fcc71314cf 63 }
WAT34 3:d2c733b52600 64 Tp.setSetPoint(ttilt);
WAT34 4:646562d80dc2 65 tilt = double(sensort.getPulses());
WAT34 4:646562d80dc2 66 tilt = tilt*61/5128.0;
WAT34 3:d2c733b52600 67 Tp.setProcessValue(tilt);
WAT34 4:646562d80dc2 68 if (abs(lo) < 0.1){
WAT34 4:646562d80dc2 69 lo = 0;
WAT34 4:646562d80dc2 70 }
WAT34 4:646562d80dc2 71 if (abs(ro) < 0.1){
WAT34 4:646562d80dc2 72 ro = 0;
WAT34 4:646562d80dc2 73 }
WAT34 10:fefff533c442 74 L= (lo-127)/127.0;
WAT34 10:fefff533c442 75 R=(ro-127)/127.0;
WAT34 3:d2c733b52600 76 ot.speed(Tp.compute());
WAT34 7:920cbfb28112 77 //pc.printf("%f %d\n\r",Tp.compute(),read);
WAT34 10:fefff533c442 78 pc.printf("%d-%f\r\n",read,lo);
WAT34 0:00fcc71314cf 79 }
WAT34 0:00fcc71314cf 80 }