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@0:00fcc71314cf, 2015-07-31 (annotated)

Committer:
WAT34
Date:
Fri Jul 31 13:42:06 2015 +0000
Revision:
0:00fcc71314cf
Child:
1:107a7d8f4c54
???????(?????)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WAT34 0:00fcc71314cf 1 #define pi 3.141592
WAT34 0:00fcc71314cf 2 #include "mbed.h"
WAT34 0:00fcc71314cf 3 #include "rtos.h"
WAT34 0:00fcc71314cf 4 #include "QEI.h"
WAT34 0:00fcc71314cf 5 #include "Motor.h"
WAT34 0:00fcc71314cf 6 #include "HMC6352.h"
WAT34 0:00fcc71314cf 7 BusOut air(p15,p16);
WAT34 0:00fcc71314cf 8 Serial conn(p29,p28);
WAT34 0:00fcc71314cf 9 BusOut mypaul(p17,p18);
WAT34 0:00fcc71314cf 10 Motor od(p21,p19,p20);
WAT34 0:00fcc71314cf 11 Motor ot(p22,p10,p11);
WAT34 0:00fcc71314cf 12 QEI sensort(p5,p6,NC,720);
WAT34 0:00fcc71314cf 13 HMC6352 sensord(p9,p10);
WAT34 0:00fcc71314cf 14 char read;
WAT34 0:00fcc71314cf 15 void serial_thread(void const *argument)
WAT34 0:00fcc71314cf 16 {
WAT34 0:00fcc71314cf 17 while(1) {
WAT34 0:00fcc71314cf 18 if(conn.readable()) {
WAT34 0:00fcc71314cf 19 read = conn.getc();
WAT34 0:00fcc71314cf 20 }
WAT34 0:00fcc71314cf 21 }
WAT34 0:00fcc71314cf 22 }
WAT34 0:00fcc71314cf 23 int main()
WAT34 0:00fcc71314cf 24 {
WAT34 0:00fcc71314cf 25 sensord.setOpMode(HMC6352_CONTINUOUS,1,20);
WAT34 0:00fcc71314cf 26 Thread thread(serial_thread);
WAT34 0:00fcc71314cf 27 double direct,tilt,cdirect,adirect,atilt,tdirect,ttilt;
WAT34 0:00fcc71314cf 28 adirect = sensord.sample()/10.0;
WAT34 0:00fcc71314cf 29 sensort.reset();
WAT34 0:00fcc71314cf 30 while(1) {
WAT34 0:00fcc71314cf 31 air = read%4;
WAT34 0:00fcc71314cf 32 mypaul = (read>>2)%4;
WAT34 0:00fcc71314cf 33 if((read>>4)%2) {
WAT34 0:00fcc71314cf 34 tdirect++;
WAT34 0:00fcc71314cf 35 }
WAT34 0:00fcc71314cf 36 if((read>>5)%2) {
WAT34 0:00fcc71314cf 37 tdirect--;
WAT34 0:00fcc71314cf 38 }
WAT34 0:00fcc71314cf 39 if((read>>6)%2) {
WAT34 0:00fcc71314cf 40 ttilt++;
WAT34 0:00fcc71314cf 41 }
WAT34 0:00fcc71314cf 42 if((read>>7)%2) {
WAT34 0:00fcc71314cf 43 ttilt--;
WAT34 0:00fcc71314cf 44 }
WAT34 0:00fcc71314cf 45 cdirect = sensord.sample()/10.0;
WAT34 0:00fcc71314cf 46 direct = cdirect+540-adirect;
WAT34 0:00fcc71314cf 47 direct = int(direct+0.5)%360;
WAT34 0:00fcc71314cf 48 direct -= 180;
WAT34 0:00fcc71314cf 49 tilt = sensort.getPulses()/360.0*2/25.0*pi;
WAT34 0:00fcc71314cf 50 od.speed(tdirect-direct);
WAT34 0:00fcc71314cf 51 ot.speed(ttilt-tilt);
WAT34 0:00fcc71314cf 52 }
WAT34 0:00fcc71314cf 53 }