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@35:2db63dec2a67, 2015-09-21 (annotated)

Committer:
DeguNaoto
Date:
Mon Sep 21 03:05:28 2015 +0000
Revision:
35:2db63dec2a67
Parent:
34:f9ef622f4376
Child:
40:5d4e6d0e2a99
20150921 ?????;

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