2015_robocon_bteam / Mbed 2 deprecated 2015robot_main_zenkoku

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

autoMode.h@25:e72246ed7ec7, 2015-11-08 (annotated)

Committer:
DeguNaoto
Date:
Sun Nov 08 05:10:15 2015 +0000
Revision:
25:e72246ed7ec7
Parent:
24:6d2573d6f2b6 
new??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:b613dc16f27d 1 #ifndef AUTOMODE_H
DeguNaoto 0:b613dc16f27d 2 #define AUTOMODE_H
DeguNaoto 0:b613dc16f27d 3
DeguNaoto 0:b613dc16f27d 4 /***PID Controller***/
DeguNaoto 0:b613dc16f27d 5 //PID velocity_controller(36.0,5274.0 ,0.0,RATE);
DeguNaoto 0:b613dc16f27d 6 //PID direction_controller(36.0,3.0,0.0,RATE);
DeguNaoto 0:b613dc16f27d 7 PID velocity_controller(9.0,5274.0,0.0,RATE);
DeguNaoto 0:b613dc16f27d 8 PID direction_controller(36.0,3.0,0.0,RATE);
DeguNaoto 0:b613dc16f27d 9
DeguNaoto 0:b613dc16f27d 10 Timeout OpStart;
DeguNaoto 0:b613dc16f27d 11
DeguNaoto 0:b613dc16f27d 12 void OpponentsStart(){
DeguNaoto 0:b613dc16f27d 13 spcount = 0.0;
DeguNaoto 0:b613dc16f27d 14 step = 15;
DeguNaoto 0:b613dc16f27d 15 CStep = 15;
DeguNaoto 0:b613dc16f27d 16 flaga = 1;
DeguNaoto 0:b613dc16f27d 17 flagf = 1;
DeguNaoto 0:b613dc16f27d 18 }
DeguNaoto 0:b613dc16f27d 19
DeguNaoto 0:b613dc16f27d 20 /***IM920 correspondence***/
DeguNaoto 0:b613dc16f27d 21 void autoIM920() {
DeguNaoto 25:e72246ed7ec7 22 if(!modeflag){ /*mode change*/
DeguNaoto 25:e72246ed7ec7 23 Com.detach();
DeguNaoto 25:e72246ed7ec7 24 autoflag=0;
DeguNaoto 25:e72246ed7ec7 25 Indicator4=0;
DeguNaoto 25:e72246ed7ec7 26 IndicatorAuto=1;
DeguNaoto 25:e72246ed7ec7 27 direction_controller.setTunings(12.0,9.0,0.0);
DeguNaoto 25:e72246ed7ec7 28 flaga=0;
DeguNaoto 25:e72246ed7ec7 29 Move_l(0.0);
DeguNaoto 25:e72246ed7ec7 30 Move_r(0.0);
DeguNaoto 25:e72246ed7ec7 31 wait(0.1);
DeguNaoto 25:e72246ed7ec7 32 mCom.attach(&mCall,RATE);
DeguNaoto 0:b613dc16f27d 33 }
DeguNaoto 0:b613dc16f27d 34 else if((b==6)&&(!flaga)){ /*start*/
DeguNaoto 0:b613dc16f27d 35 if(edge6){
DeguNaoto 0:b613dc16f27d 36 edge6=0;
DeguNaoto 0:b613dc16f27d 37 resetState();
DeguNaoto 0:b613dc16f27d 38 flagf=1;
DeguNaoto 0:b613dc16f27d 39 spcount=0.0;
DeguNaoto 2:738b28f6a04b 40 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 41 // targ_velocity=speed;
DeguNaoto 0:b613dc16f27d 42 #ifdef BLUE
DeguNaoto 17:726b6f53a457 43 sendData(5,69); //right
DeguNaoto 0:b613dc16f27d 44 wait(0.05);
DeguNaoto 0:b613dc16f27d 45 sendData(4,69); //left
DeguNaoto 17:726b6f53a457 46 wait(0.05);
DeguNaoto 17:726b6f53a457 47 sendData(6,59); //middle
DeguNaoto 0:b613dc16f27d 48 stateR = 69;
DeguNaoto 0:b613dc16f27d 49 stateL = 69;
DeguNaoto 0:b613dc16f27d 50 #else
DeguNaoto 0:b613dc16f27d 51 sendData(5,69); //right
DeguNaoto 0:b613dc16f27d 52 wait(0.05);
DeguNaoto 0:b613dc16f27d 53 sendData(4,69); //left
DeguNaoto 17:726b6f53a457 54 wait(0.05);
DeguNaoto 17:726b6f53a457 55 sendData(6,59); //middle
DeguNaoto 0:b613dc16f27d 56 stateR = 69;
DeguNaoto 0:b613dc16f27d 57 stateL = 69;
DeguNaoto 0:b613dc16f27d 58 #endif
DeguNaoto 0:b613dc16f27d 59 step = 0;
DeguNaoto 0:b613dc16f27d 60 CStep = 1;
DeguNaoto 0:b613dc16f27d 61 flaga = 1;
DeguNaoto 0:b613dc16f27d 62 }
DeguNaoto 0:b613dc16f27d 63 }
DeguNaoto 21:bdf8ac5c200c 64 /*if(a2){
DeguNaoto 0:b613dc16f27d 65 skip = 1;
DeguNaoto 0:b613dc16f27d 66 }
DeguNaoto 0:b613dc16f27d 67 else if(!a2){
DeguNaoto 0:b613dc16f27d 68 skip = 0;
DeguNaoto 21:bdf8ac5c200c 69 }*/
DeguNaoto 21:bdf8ac5c200c 70 skip=0;
DeguNaoto 0:b613dc16f27d 71 if(b!=1) edge1=1;
DeguNaoto 0:b613dc16f27d 72 if(b!=2) edge2=1;
DeguNaoto 0:b613dc16f27d 73 if(b!=3) edge3=1;
DeguNaoto 0:b613dc16f27d 74 if(b!=4) edge4=1;
DeguNaoto 0:b613dc16f27d 75 if(b!=5) edge5=1;
DeguNaoto 0:b613dc16f27d 76 if(b!=6) edge6=1;
DeguNaoto 0:b613dc16f27d 77 if(b!=7) edge7=1;
DeguNaoto 0:b613dc16f27d 78 if(b!=8) edge8=1;
DeguNaoto 0:b613dc16f27d 79 if(b!=9) edge9=1;
DeguNaoto 0:b613dc16f27d 80 }
DeguNaoto 0:b613dc16f27d 81 /***The function is PID controller initialize.***/
DeguNaoto 0:b613dc16f27d 82 inline void initializeControllers()
DeguNaoto 0:b613dc16f27d 83 {
DeguNaoto 0:b613dc16f27d 84 velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
DeguNaoto 6:ca3a74a93ae2 85 // velocity_controller.setInputLimits(-1500.0, 1500.0); //x1
DeguNaoto 2:738b28f6a04b 86 direction_controller.setInputLimits(-PI, PI); //x2
DeguNaoto 0:b613dc16f27d 87
DeguNaoto 0:b613dc16f27d 88 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 0:b613dc16f27d 89 velocity_controller.setOutputLimits(0.0, 1.0);
DeguNaoto 0:b613dc16f27d 90 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 0:b613dc16f27d 91
DeguNaoto 0:b613dc16f27d 92 //set bias. 初期値
DeguNaoto 0:b613dc16f27d 93 velocity_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 94 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 95
DeguNaoto 0:b613dc16f27d 96 //set mode.
DeguNaoto 0:b613dc16f27d 97 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 0:b613dc16f27d 98 direction_controller.setMode(AUTO_MODE);
DeguNaoto 0:b613dc16f27d 99 }
DeguNaoto 0:b613dc16f27d 100
DeguNaoto 0:b613dc16f27d 101 /***The function is following move speed.***/
DeguNaoto 0:b613dc16f27d 102 inline void velocity_following()
DeguNaoto 0:b613dc16f27d 103 {
DeguNaoto 0:b613dc16f27d 104 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 0:b613dc16f27d 105 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 0:b613dc16f27d 106 x1 = (double)velocity_controller.compute();
DeguNaoto 0:b613dc16f27d 107 }
DeguNaoto 0:b613dc16f27d 108
DeguNaoto 0:b613dc16f27d 109 inline void sita_following()
DeguNaoto 0:b613dc16f27d 110 {
DeguNaoto 0:b613dc16f27d 111 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 0:b613dc16f27d 112 direction_controller.setProcessValue((float)sita);
DeguNaoto 0:b613dc16f27d 113 x2 = (double)direction_controller.compute();
DeguNaoto 0:b613dc16f27d 114 }
DeguNaoto 0:b613dc16f27d 115
DeguNaoto 0:b613dc16f27d 116 inline void move_following()
DeguNaoto 0:b613dc16f27d 117 {
DeguNaoto 0:b613dc16f27d 118 velocity_following();
DeguNaoto 0:b613dc16f27d 119 sita_following();
DeguNaoto 0:b613dc16f27d 120
DeguNaoto 0:b613dc16f27d 121 if(flagf==0){
DeguNaoto 2:738b28f6a04b 122 // Vr = ( 2.0*(-x1) + x2 ) / 3.0;
DeguNaoto 2:738b28f6a04b 123 // Vl = ( 2.0*(-x1) - x2 ) / 3.0;
DeguNaoto 3:8d8c25c556ae 124 /*if(x2>0.0){
DeguNaoto 2:738b28f6a04b 125 Vr = -x1;
DeguNaoto 2:738b28f6a04b 126 Vl = -x1 - x2;
DeguNaoto 2:738b28f6a04b 127 }
DeguNaoto 2:738b28f6a04b 128 else{
DeguNaoto 2:738b28f6a04b 129 Vr = -x1 + x2;
DeguNaoto 2:738b28f6a04b 130 Vl = -x1;
DeguNaoto 3:8d8c25c556ae 131 }*/
DeguNaoto 3:8d8c25c556ae 132 if(x2>0.0){
DeguNaoto 3:8d8c25c556ae 133 Vr = -x1 + x2;
DeguNaoto 3:8d8c25c556ae 134 Vl = -x1;
DeguNaoto 3:8d8c25c556ae 135 }
DeguNaoto 3:8d8c25c556ae 136 else{
DeguNaoto 3:8d8c25c556ae 137 Vr = -x1;
DeguNaoto 3:8d8c25c556ae 138 Vl = -x1 - x2;
DeguNaoto 2:738b28f6a04b 139 }
DeguNaoto 0:b613dc16f27d 140 }
DeguNaoto 0:b613dc16f27d 141 else if(flagf==1){
DeguNaoto 2:738b28f6a04b 142 if(x2>0.0){
DeguNaoto 0:b613dc16f27d 143 Vr = x1;
DeguNaoto 0:b613dc16f27d 144 Vl = x1 - x2;
DeguNaoto 0:b613dc16f27d 145 }
DeguNaoto 0:b613dc16f27d 146 else{
DeguNaoto 0:b613dc16f27d 147 Vr = x1 + x2;
DeguNaoto 0:b613dc16f27d 148 Vl = x1;
DeguNaoto 0:b613dc16f27d 149 }
DeguNaoto 0:b613dc16f27d 150 }
DeguNaoto 0:b613dc16f27d 151 Move_r( ( float ) Vr );
DeguNaoto 0:b613dc16f27d 152 Move_l( ( float ) Vl );
DeguNaoto 0:b613dc16f27d 153 }
DeguNaoto 0:b613dc16f27d 154
DeguNaoto 0:b613dc16f27d 155 #endif /*autoMode.h*/