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@26:8e6c736b6791, 2015-09-18 (annotated)

Committer:
DeguNaoto
Date:
Fri Sep 18 04:00:33 2015 +0000
Revision:
26:8e6c736b6791
Parent:
22:3996c3f41922
Child:
28:c6ed6fb95795
??

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 26:8e6c736b6791 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 22:3996c3f41922 14 else if(r1) targ_sita=0.0;
DeguNaoto 22:3996c3f41922 15 else if(l1) targ_sita=PI/2.0;
DeguNaoto 22:3996c3f41922 16 else if(square) if(edge_square) edge_square=0,autoflag=0,Indicator4=0;
DeguNaoto 22:3996c3f41922 17 else if(cross){
DeguNaoto 22:3996c3f41922 18 sendData(1, 4);
DeguNaoto 22:3996c3f41922 19 sita=PI/4.0,x=0.0,y=0.0;
DeguNaoto 22:3996c3f41922 20 targ_velocity=0.0;
DeguNaoto 22:3996c3f41922 21 targ_sita=PI/4.0;
DeguNaoto 22:3996c3f41922 22 velocity_controller.reset();
DeguNaoto 22:3996c3f41922 23 direction_controller.reset();
DeguNaoto 22:3996c3f41922 24 wait(0.3);
DeguNaoto 22:3996c3f41922 25 sita=PI/4.0,x=0.0,y=0.0;
DeguNaoto 22:3996c3f41922 26 velocity_controller.reset();
DeguNaoto 22:3996c3f41922 27 direction_controller.reset();
DeguNaoto 22:3996c3f41922 28 }
DeguNaoto 22:3996c3f41922 29 else if(r2){
DeguNaoto 22:3996c3f41922 30 if(edge_r1){
DeguNaoto 22:3996c3f41922 31 edge_r1=0;
DeguNaoto 22:3996c3f41922 32 sendData(1, 1);
DeguNaoto 22:3996c3f41922 33 wait(0.1);
DeguNaoto 22:3996c3f41922 34 sendData(1, 4);
DeguNaoto 22:3996c3f41922 35 wait(0.01);
DeguNaoto 22:3996c3f41922 36 sendData(1, 2);
DeguNaoto 22:3996c3f41922 37 wait(0.1);
DeguNaoto 22:3996c3f41922 38 sendData(1, 4);
DeguNaoto 22:3996c3f41922 39 }
DeguNaoto 22:3996c3f41922 40 }
DeguNaoto 22:3996c3f41922 41 else if(l2){
DeguNaoto 22:3996c3f41922 42 if(edge_l1){
DeguNaoto 22:3996c3f41922 43 edge_l1=0;
DeguNaoto 22:3996c3f41922 44 sendData(1, 3);
DeguNaoto 22:3996c3f41922 45 wait(0.1);
DeguNaoto 22:3996c3f41922 46 sendData(1, 4);
DeguNaoto 22:3996c3f41922 47 wait(0.01);
DeguNaoto 22:3996c3f41922 48 sendData(1, 5);
DeguNaoto 22:3996c3f41922 49 wait(0.1);
DeguNaoto 22:3996c3f41922 50 sendData(1, 7);
DeguNaoto 22:3996c3f41922 51 }
DeguNaoto 22:3996c3f41922 52 }
DeguNaoto 22:3996c3f41922 53 else if(triangle){
DeguNaoto 22:3996c3f41922 54 if(edge_triangle){
DeguNaoto 22:3996c3f41922 55 edge_triangle=0;
DeguNaoto 22:3996c3f41922 56 if(cylinder_step==3) cylinder_step=0;
DeguNaoto 22:3996c3f41922 57 cylinder_step++;
DeguNaoto 22:3996c3f41922 58 sendData(1, cylinder_step);
DeguNaoto 22:3996c3f41922 59 }
DeguNaoto 26:8e6c736b6791 60 }*/
DeguNaoto 22:3996c3f41922 61 }
DeguNaoto 22:3996c3f41922 62
DeguNaoto 22:3996c3f41922 63
DeguNaoto 22:3996c3f41922 64
DeguNaoto 22:3996c3f41922 65
DeguNaoto 21:79b94cb922f0 66 /***The function is PID controller initialize.***/
DeguNaoto 21:79b94cb922f0 67 inline void initializeControllers()
DeguNaoto 21:79b94cb922f0 68 {
DeguNaoto 21:79b94cb922f0 69 velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 21:79b94cb922f0 70 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 21:79b94cb922f0 71
DeguNaoto 21:79b94cb922f0 72 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 21:79b94cb922f0 73 velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 21:79b94cb922f0 74 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 21:79b94cb922f0 75
DeguNaoto 21:79b94cb922f0 76 //set bias. 初期値
DeguNaoto 21:79b94cb922f0 77 velocity_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 78 direction_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 79
DeguNaoto 21:79b94cb922f0 80 //set mode.
DeguNaoto 21:79b94cb922f0 81 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 82 direction_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 83 }
DeguNaoto 21:79b94cb922f0 84
DeguNaoto 21:79b94cb922f0 85 /***The function is following move speed.***/
DeguNaoto 21:79b94cb922f0 86 inline void velocity_following()
DeguNaoto 21:79b94cb922f0 87 {
DeguNaoto 21:79b94cb922f0 88 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 21:79b94cb922f0 89 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 21:79b94cb922f0 90 x1 = (double)velocity_controller.compute();
DeguNaoto 21:79b94cb922f0 91 }
DeguNaoto 21:79b94cb922f0 92
DeguNaoto 21:79b94cb922f0 93 inline void sita_following()
DeguNaoto 21:79b94cb922f0 94 {
DeguNaoto 21:79b94cb922f0 95 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 21:79b94cb922f0 96 direction_controller.setProcessValue((float)sita);
DeguNaoto 21:79b94cb922f0 97 //direction_controller.setSetPoint(0.0); //目標値とのずれをなくす
DeguNaoto 21:79b94cb922f0 98 //direction_controller.setProcessValue(-y);
DeguNaoto 21:79b94cb922f0 99 x2 = (double)direction_controller.compute();
DeguNaoto 21:79b94cb922f0 100 }
DeguNaoto 21:79b94cb922f0 101
DeguNaoto 21:79b94cb922f0 102 inline void move_following()
DeguNaoto 21:79b94cb922f0 103 {
DeguNaoto 21:79b94cb922f0 104 velocity_following();
DeguNaoto 21:79b94cb922f0 105 sita_following();
DeguNaoto 21:79b94cb922f0 106 Vr = ( x1 + x2 ) / 2.0;
DeguNaoto 21:79b94cb922f0 107 Vl = ( x1 - x2 ) / 2.0;
DeguNaoto 21:79b94cb922f0 108 if( abs(Vr) < 0.05 ) Vr = 0.0;
DeguNaoto 21:79b94cb922f0 109 if( abs(Vl) < 0.05 ) Vl = 0.0;
DeguNaoto 21:79b94cb922f0 110 Move_r( ( float ) Vr );
DeguNaoto 21:79b94cb922f0 111 Move_l( ( float ) Vl );
DeguNaoto 21:79b94cb922f0 112 }
DeguNaoto 21:79b94cb922f0 113
DeguNaoto 21:79b94cb922f0 114 #endif /*autoMode.h*/