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@47:46db7f076cea, 2015-09-25 (annotated)

Committer:
DeguNaoto
Date:
Fri Sep 25 01:25:22 2015 +0000
Revision:
47:46db7f076cea
Parent:
40:5d4e6d0e2a99
Child:
51:cb430192b28b
????????;

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