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@58:6b73e683fa70, 2015-09-30 (annotated)

Committer:
DeguNaoto
Date:
Wed Sep 30 12:31:48 2015 +0000
Revision:
58:6b73e683fa70
Parent:
57:3fbd487e055e 
????????????????

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 57:3fbd487e055e 8 #ifdef IM920
DeguNaoto 52:d9e1629da852 9 /***IM920 correspondence***/
DeguNaoto 52:d9e1629da852 10 void autoIM920() {
DeguNaoto 52:d9e1629da852 11 if(b==7){ /*mode change*/
DeguNaoto 52:d9e1629da852 12 if(edge7){
DeguNaoto 52:d9e1629da852 13 edge7=0;
DeguNaoto 52:d9e1629da852 14 autoflag=0;
DeguNaoto 52:d9e1629da852 15 Indicator4=0;
DeguNaoto 52:d9e1629da852 16 IndicatorAuto=1;
DeguNaoto 52:d9e1629da852 17 }
DeguNaoto 52:d9e1629da852 18 }
DeguNaoto 52:d9e1629da852 19 else if(b==6){ /*start*/
DeguNaoto 52:d9e1629da852 20 targ_velocity=speed;
DeguNaoto 52:d9e1629da852 21 sendData(4,defoR);
DeguNaoto 52:d9e1629da852 22 wait(0.1);
DeguNaoto 52:d9e1629da852 23 sendData(5,defoL);
DeguNaoto 52:d9e1629da852 24 wait(0.1);
DeguNaoto 52:d9e1629da852 25 stateR = defoR;
DeguNaoto 52:d9e1629da852 26 stateL = defoL;
DeguNaoto 58:6b73e683fa70 27 step=0;
DeguNaoto 58:6b73e683fa70 28 CStep=1;
DeguNaoto 52:d9e1629da852 29 }
DeguNaoto 58:6b73e683fa70 30 else if(b==5){ /*middle pole start*/
DeguNaoto 58:6b73e683fa70 31 targ_velocity=speed;
DeguNaoto 58:6b73e683fa70 32 sendData(4,defoR);
DeguNaoto 52:d9e1629da852 33 wait(0.1);
DeguNaoto 58:6b73e683fa70 34 sendData(5,defoL);
DeguNaoto 52:d9e1629da852 35 wait(0.1);
DeguNaoto 58:6b73e683fa70 36 stateR = defoR;
DeguNaoto 58:6b73e683fa70 37 stateL = defoL;
DeguNaoto 58:6b73e683fa70 38 step=4;
DeguNaoto 52:d9e1629da852 39 }
DeguNaoto 52:d9e1629da852 40 else if(b==8){ /*set defo*/
DeguNaoto 57:3fbd487e055e 41 sendData(5,defoR);
DeguNaoto 52:d9e1629da852 42 wait(0.1);
DeguNaoto 57:3fbd487e055e 43 sendData(4,defoL);
DeguNaoto 52:d9e1629da852 44 wait(0.1);
DeguNaoto 52:d9e1629da852 45 }
DeguNaoto 52:d9e1629da852 46 else if(b==9){ /*stop*/
DeguNaoto 58:6b73e683fa70 47 sendData(5,31);
DeguNaoto 58:6b73e683fa70 48 wait(0.1);
DeguNaoto 58:6b73e683fa70 49 sendData(4,31);
DeguNaoto 58:6b73e683fa70 50 wait(0.1);
DeguNaoto 29:03bb1ddbe456 51 Motor_swing=0;
DeguNaoto 22:3996c3f41922 52 sita=PI/4.0,x=0.0,y=0.0;
DeguNaoto 22:3996c3f41922 53 targ_velocity=0.0;
DeguNaoto 22:3996c3f41922 54 targ_sita=PI/4.0;
DeguNaoto 22:3996c3f41922 55 velocity_controller.reset();
DeguNaoto 22:3996c3f41922 56 direction_controller.reset();
DeguNaoto 52:d9e1629da852 57 }
DeguNaoto 52:d9e1629da852 58 else if(b==1){ /*L down*/
DeguNaoto 52:d9e1629da852 59 if(edge1){
DeguNaoto 52:d9e1629da852 60 edge1=0;
DeguNaoto 52:d9e1629da852 61 if(stateL!=1) stateL--;
DeguNaoto 57:3fbd487e055e 62 sendData(4,stateL);
DeguNaoto 52:d9e1629da852 63 }
DeguNaoto 52:d9e1629da852 64 }
DeguNaoto 52:d9e1629da852 65 else if(b==2){ /*L up*/
DeguNaoto 52:d9e1629da852 66 if(edge2){
DeguNaoto 52:d9e1629da852 67 edge2=0;
DeguNaoto 52:d9e1629da852 68 if(stateL!=28) stateL++;
DeguNaoto 57:3fbd487e055e 69 sendData(4,stateL);
DeguNaoto 22:3996c3f41922 70 }
DeguNaoto 52:d9e1629da852 71 }
DeguNaoto 52:d9e1629da852 72 else if(b==3){ /*R down*/
DeguNaoto 52:d9e1629da852 73 if(edge3){
DeguNaoto 52:d9e1629da852 74 edge3=0;
DeguNaoto 52:d9e1629da852 75 if(stateR!=1) stateR--;
DeguNaoto 57:3fbd487e055e 76 sendData(5,stateR);
DeguNaoto 52:d9e1629da852 77 }
DeguNaoto 52:d9e1629da852 78 }
DeguNaoto 52:d9e1629da852 79 else if(b==4){ /*R up*/
DeguNaoto 52:d9e1629da852 80 if(edge4){
DeguNaoto 52:d9e1629da852 81 edge4=0;
DeguNaoto 52:d9e1629da852 82 if(stateR!=28) stateR++;
DeguNaoto 57:3fbd487e055e 83 sendData(5,stateR);
DeguNaoto 52:d9e1629da852 84 }
DeguNaoto 52:d9e1629da852 85 }
DeguNaoto 52:d9e1629da852 86 if(b!=7) edge7=1;
DeguNaoto 52:d9e1629da852 87 if(b!=1) edge1=1;
DeguNaoto 52:d9e1629da852 88 if(b!=2) edge2=1;
DeguNaoto 52:d9e1629da852 89 if(b!=3) edge3=1;
DeguNaoto 52:d9e1629da852 90 if(b!=4) edge4=1;
DeguNaoto 22:3996c3f41922 91 }
DeguNaoto 57:3fbd487e055e 92 #else
DeguNaoto 57:3fbd487e055e 93 void autoPS3(){
DeguNaoto 57:3fbd487e055e 94 if(circle){ /*mode change*/
DeguNaoto 57:3fbd487e055e 95 if(edge_circle){
DeguNaoto 57:3fbd487e055e 96 edge_circle=0;
DeguNaoto 57:3fbd487e055e 97 autoflag=0;
DeguNaoto 57:3fbd487e055e 98 Indicator4=0;
DeguNaoto 57:3fbd487e055e 99 IndicatorAuto=1;
DeguNaoto 57:3fbd487e055e 100 }
DeguNaoto 57:3fbd487e055e 101 }
DeguNaoto 57:3fbd487e055e 102 else if(up){ /*start*/
DeguNaoto 57:3fbd487e055e 103 targ_velocity=speed;
DeguNaoto 57:3fbd487e055e 104 sendData(5,defoR);
DeguNaoto 57:3fbd487e055e 105 wait(0.1);
DeguNaoto 57:3fbd487e055e 106 sendData(4,defoL);
DeguNaoto 57:3fbd487e055e 107 wait(0.1);
DeguNaoto 57:3fbd487e055e 108 stateR = defoR;
DeguNaoto 57:3fbd487e055e 109 stateL = defoL;
DeguNaoto 58:6b73e683fa70 110 step=0;
DeguNaoto 58:6b73e683fa70 111 CStep=1;
DeguNaoto 58:6b73e683fa70 112 }
DeguNaoto 58:6b73e683fa70 113 else if(left) { /*middle start*/
DeguNaoto 58:6b73e683fa70 114 targ_velocity=speed;
DeguNaoto 58:6b73e683fa70 115 sendData(4,15);
DeguNaoto 58:6b73e683fa70 116 wait(0.1);
DeguNaoto 58:6b73e683fa70 117 sendData(5,15);
DeguNaoto 58:6b73e683fa70 118 wait(0.1);
DeguNaoto 58:6b73e683fa70 119 stateR = defoR;
DeguNaoto 58:6b73e683fa70 120 stateL = defoL;
DeguNaoto 58:6b73e683fa70 121 step=4;
DeguNaoto 58:6b73e683fa70 122 CStep=6;
DeguNaoto 57:3fbd487e055e 123 }
DeguNaoto 57:3fbd487e055e 124 else if(square){ /*reset*/
DeguNaoto 57:3fbd487e055e 125 sendData(4,31);
DeguNaoto 57:3fbd487e055e 126 wait(0.1);
DeguNaoto 57:3fbd487e055e 127 sendData(5,31);
DeguNaoto 57:3fbd487e055e 128 wait(0.1);
DeguNaoto 57:3fbd487e055e 129 }
DeguNaoto 58:6b73e683fa70 130 /*else if(triangle){
DeguNaoto 57:3fbd487e055e 131 sendData(5,defoL);
DeguNaoto 57:3fbd487e055e 132 wait(0.1);
DeguNaoto 57:3fbd487e055e 133 sendData(4,defoR);
DeguNaoto 57:3fbd487e055e 134 wait(0.1);
DeguNaoto 58:6b73e683fa70 135 }*/
DeguNaoto 57:3fbd487e055e 136 else if(cross){ /*stop*/
DeguNaoto 58:6b73e683fa70 137 sendData(5,31);
DeguNaoto 58:6b73e683fa70 138 wait(0.1);
DeguNaoto 58:6b73e683fa70 139 sendData(4,31);
DeguNaoto 58:6b73e683fa70 140 wait(0.1);
DeguNaoto 57:3fbd487e055e 141 Motor_swing=0;
DeguNaoto 57:3fbd487e055e 142 sita=PI/4.0,x=0.0,y=0.0;
DeguNaoto 57:3fbd487e055e 143 targ_velocity=0.0;
DeguNaoto 57:3fbd487e055e 144 targ_sita=PI/4.0;
DeguNaoto 57:3fbd487e055e 145 velocity_controller.reset();
DeguNaoto 57:3fbd487e055e 146 direction_controller.reset();
DeguNaoto 57:3fbd487e055e 147 }
DeguNaoto 57:3fbd487e055e 148 else if(l1){ /*L up*/
DeguNaoto 57:3fbd487e055e 149 if(edge_l1){
DeguNaoto 57:3fbd487e055e 150 edge_l1=0;
DeguNaoto 57:3fbd487e055e 151 if(stateL!=2) stateL--;
DeguNaoto 57:3fbd487e055e 152 sendData(4,stateL);
DeguNaoto 57:3fbd487e055e 153 }
DeguNaoto 57:3fbd487e055e 154 }
DeguNaoto 57:3fbd487e055e 155 else if(l2){ /*L down*/
DeguNaoto 57:3fbd487e055e 156 if(edge_l2){
DeguNaoto 57:3fbd487e055e 157 edge_l2=0;
DeguNaoto 57:3fbd487e055e 158 if(stateL!=28) stateL++;
DeguNaoto 57:3fbd487e055e 159 sendData(4,stateL);
DeguNaoto 57:3fbd487e055e 160 }
DeguNaoto 57:3fbd487e055e 161 }
DeguNaoto 57:3fbd487e055e 162 else if(r1){ /*R up*/
DeguNaoto 57:3fbd487e055e 163 if(edge_r1){
DeguNaoto 57:3fbd487e055e 164 edge_r1=0;
DeguNaoto 57:3fbd487e055e 165 if(stateR!=2) stateR--;
DeguNaoto 57:3fbd487e055e 166 sendData(5,stateR);
DeguNaoto 57:3fbd487e055e 167 }
DeguNaoto 57:3fbd487e055e 168 }
DeguNaoto 57:3fbd487e055e 169 else if(r2){ /*R down*/
DeguNaoto 57:3fbd487e055e 170 if(edge_r2){
DeguNaoto 57:3fbd487e055e 171 edge_r2=0;
DeguNaoto 57:3fbd487e055e 172 if(stateR!=28) stateR++;
DeguNaoto 57:3fbd487e055e 173 sendData(5,stateR);
DeguNaoto 57:3fbd487e055e 174 }
DeguNaoto 57:3fbd487e055e 175 }
DeguNaoto 57:3fbd487e055e 176 }
DeguNaoto 57:3fbd487e055e 177 #endif
DeguNaoto 21:79b94cb922f0 178 /***The function is PID controller initialize.***/
DeguNaoto 21:79b94cb922f0 179 inline void initializeControllers()
DeguNaoto 21:79b94cb922f0 180 {
DeguNaoto 21:79b94cb922f0 181 velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 21:79b94cb922f0 182 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 21:79b94cb922f0 183
DeguNaoto 21:79b94cb922f0 184 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 21:79b94cb922f0 185 velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 21:79b94cb922f0 186 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 31:74e77ef0831c 187
DeguNaoto 21:79b94cb922f0 188 //set bias. 初期値
DeguNaoto 21:79b94cb922f0 189 velocity_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 190 direction_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 191
DeguNaoto 21:79b94cb922f0 192 //set mode.
DeguNaoto 21:79b94cb922f0 193 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 194 direction_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 195 }
DeguNaoto 21:79b94cb922f0 196
DeguNaoto 21:79b94cb922f0 197 /***The function is following move speed.***/
DeguNaoto 21:79b94cb922f0 198 inline void velocity_following()
DeguNaoto 21:79b94cb922f0 199 {
DeguNaoto 21:79b94cb922f0 200 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 21:79b94cb922f0 201 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 21:79b94cb922f0 202 x1 = (double)velocity_controller.compute();
DeguNaoto 21:79b94cb922f0 203 }
DeguNaoto 21:79b94cb922f0 204
DeguNaoto 21:79b94cb922f0 205 inline void sita_following()
DeguNaoto 21:79b94cb922f0 206 {
DeguNaoto 21:79b94cb922f0 207 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 21:79b94cb922f0 208 direction_controller.setProcessValue((float)sita);
DeguNaoto 55:db1797ac6cb1 209 //direction_controller.setSetPoint(0.0); /*目標値とのずれをなくす*/
DeguNaoto 21:79b94cb922f0 210 //direction_controller.setProcessValue(-y);
DeguNaoto 21:79b94cb922f0 211 x2 = (double)direction_controller.compute();
DeguNaoto 21:79b94cb922f0 212 }
DeguNaoto 21:79b94cb922f0 213
DeguNaoto 21:79b94cb922f0 214 inline void move_following()
DeguNaoto 21:79b94cb922f0 215 {
DeguNaoto 21:79b94cb922f0 216 velocity_following();
DeguNaoto 21:79b94cb922f0 217 sita_following();
DeguNaoto 21:79b94cb922f0 218 Vr = ( x1 + x2 ) / 2.0;
DeguNaoto 21:79b94cb922f0 219 Vl = ( x1 - x2 ) / 2.0;
DeguNaoto 21:79b94cb922f0 220 if( abs(Vr) < 0.05 ) Vr = 0.0;
DeguNaoto 21:79b94cb922f0 221 if( abs(Vl) < 0.05 ) Vl = 0.0;
DeguNaoto 21:79b94cb922f0 222 Move_r( ( float ) Vr );
DeguNaoto 21:79b94cb922f0 223 Move_l( ( float ) Vl );
DeguNaoto 21:79b94cb922f0 224 }
DeguNaoto 21:79b94cb922f0 225
DeguNaoto 21:79b94cb922f0 226 #endif /*autoMode.h*/