2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

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:   PID QEI mbed

Fork of 2015robot_main by Naoto Deguchi

autoMode.h@29:03bb1ddbe456, 2015-09-18 (annotated)

Committer:
DeguNaoto
Date:
Fri Sep 18 08:55:05 2015 +0000
Revision:
29:03bb1ddbe456
Parent:
28:c6ed6fb95795
Child:
30:2923c6879e5f
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 21:79b94cb922f0 1 #ifndef AUTOMODE_H
DeguNaoto 21:79b94cb922f0 2 #define AUTOMODE_H
DeguNaoto 21:79b94cb922f0 3
DeguNaoto 21:79b94cb922f0 4 /***PID Controller***/
DeguNaoto 21:79b94cb922f0 5 PID velocity_controller(18.0,5274.0 ,0.0,RATE);
DeguNaoto 21:79b94cb922f0 6 PID direction_controller(9.0,2637.0,0.0,RATE);
DeguNaoto 21:79b94cb922f0 7
DeguNaoto 22:3996c3f41922 8 /***Ps3 correspondence***/
DeguNaoto 22:3996c3f41922 9 void autoPs3(){
DeguNaoto 28:c6ed6fb95795 10 if(up) targ_velocity=speed;
DeguNaoto 22:3996c3f41922 11 else if(down) targ_velocity=-speed;
DeguNaoto 22:3996c3f41922 12 else if(right) targ_velocity=0.0,targ_sita=0.0;
DeguNaoto 22:3996c3f41922 13 else if(left) targ_velocity=0.0,targ_sita=PI/2.0;
DeguNaoto 29:03bb1ddbe456 14 else if(r1) targ_sita=0.0;
DeguNaoto 29:03bb1ddbe456 15 else if(l1) targ_sita=PI/2.0;
DeguNaoto 29:03bb1ddbe456 16 else if(circle){
DeguNaoto 29:03bb1ddbe456 17 if(edge_circle) edge_circle=0,autoflag=0,Indicator4=0;
DeguNaoto 28:c6ed6fb95795 18 }
DeguNaoto 22:3996c3f41922 19 else if(cross){
DeguNaoto 29:03bb1ddbe456 20 Motor_swing=0;
DeguNaoto 22:3996c3f41922 21 sita=PI/4.0,x=0.0,y=0.0;
DeguNaoto 22:3996c3f41922 22 targ_velocity=0.0;
DeguNaoto 22:3996c3f41922 23 targ_sita=PI/4.0;
DeguNaoto 22:3996c3f41922 24 velocity_controller.reset();
DeguNaoto 22:3996c3f41922 25 direction_controller.reset();
DeguNaoto 22:3996c3f41922 26 }
DeguNaoto 28:c6ed6fb95795 27 else if(triangle==1){
DeguNaoto 28:c6ed6fb95795 28 if(edge_triangle){
DeguNaoto 28:c6ed6fb95795 29 edge_triangle=0;
DeguNaoto 28:c6ed6fb95795 30 if(cylinderStep==3) cylinderStep=0;
DeguNaoto 28:c6ed6fb95795 31 cylinderStep++;
DeguNaoto 28:c6ed6fb95795 32 sendData(1, cylinderStep);
DeguNaoto 22:3996c3f41922 33 }
DeguNaoto 22:3996c3f41922 34 }
DeguNaoto 22:3996c3f41922 35 }
DeguNaoto 22:3996c3f41922 36
DeguNaoto 22:3996c3f41922 37
DeguNaoto 22:3996c3f41922 38
DeguNaoto 22:3996c3f41922 39
DeguNaoto 21:79b94cb922f0 40 /***The function is PID controller initialize.***/
DeguNaoto 21:79b94cb922f0 41 inline void initializeControllers()
DeguNaoto 21:79b94cb922f0 42 {
DeguNaoto 21:79b94cb922f0 43 velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 21:79b94cb922f0 44 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 21:79b94cb922f0 45
DeguNaoto 21:79b94cb922f0 46 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 21:79b94cb922f0 47 velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 21:79b94cb922f0 48 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 21:79b94cb922f0 49
DeguNaoto 21:79b94cb922f0 50 //set bias. 初期値
DeguNaoto 21:79b94cb922f0 51 velocity_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 52 direction_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 53
DeguNaoto 21:79b94cb922f0 54 //set mode.
DeguNaoto 21:79b94cb922f0 55 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 56 direction_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 57 }
DeguNaoto 21:79b94cb922f0 58
DeguNaoto 21:79b94cb922f0 59 /***The function is following move speed.***/
DeguNaoto 21:79b94cb922f0 60 inline void velocity_following()
DeguNaoto 21:79b94cb922f0 61 {
DeguNaoto 21:79b94cb922f0 62 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 21:79b94cb922f0 63 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 21:79b94cb922f0 64 x1 = (double)velocity_controller.compute();
DeguNaoto 21:79b94cb922f0 65 }
DeguNaoto 21:79b94cb922f0 66
DeguNaoto 21:79b94cb922f0 67 inline void sita_following()
DeguNaoto 21:79b94cb922f0 68 {
DeguNaoto 21:79b94cb922f0 69 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 21:79b94cb922f0 70 direction_controller.setProcessValue((float)sita);
DeguNaoto 21:79b94cb922f0 71 //direction_controller.setSetPoint(0.0); //目標値とのずれをなくす
DeguNaoto 21:79b94cb922f0 72 //direction_controller.setProcessValue(-y);
DeguNaoto 21:79b94cb922f0 73 x2 = (double)direction_controller.compute();
DeguNaoto 21:79b94cb922f0 74 }
DeguNaoto 21:79b94cb922f0 75
DeguNaoto 21:79b94cb922f0 76 inline void move_following()
DeguNaoto 21:79b94cb922f0 77 {
DeguNaoto 21:79b94cb922f0 78 velocity_following();
DeguNaoto 21:79b94cb922f0 79 sita_following();
DeguNaoto 21:79b94cb922f0 80 Vr = ( x1 + x2 ) / 2.0;
DeguNaoto 21:79b94cb922f0 81 Vl = ( x1 - x2 ) / 2.0;
DeguNaoto 21:79b94cb922f0 82 if( abs(Vr) < 0.05 ) Vr = 0.0;
DeguNaoto 21:79b94cb922f0 83 if( abs(Vl) < 0.05 ) Vl = 0.0;
DeguNaoto 21:79b94cb922f0 84 Move_r( ( float ) Vr );
DeguNaoto 21:79b94cb922f0 85 Move_l( ( float ) Vl );
DeguNaoto 21:79b94cb922f0 86 }
DeguNaoto 21:79b94cb922f0 87
DeguNaoto 21:79b94cb922f0 88 #endif /*autoMode.h*/