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@55:db1797ac6cb1, 2015-09-28 (annotated)

Committer:
DeguNaoto
Date:
Mon Sep 28 07:53:39 2015 +0000
Revision:
55:db1797ac6cb1
Parent:
52:d9e1629da852
Child:
57:3fbd487e055e
????????????????(3450ppr)

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 52:d9e1629da852 8 /***IM920 correspondence***/
DeguNaoto 52:d9e1629da852 9 void autoIM920() {
DeguNaoto 52:d9e1629da852 10 if(b==7){ /*mode change*/
DeguNaoto 52:d9e1629da852 11 if(edge7){
DeguNaoto 52:d9e1629da852 12 edge7=0;
DeguNaoto 52:d9e1629da852 13 autoflag=0;
DeguNaoto 52:d9e1629da852 14 Indicator4=0;
DeguNaoto 52:d9e1629da852 15 IndicatorAuto=1;
DeguNaoto 52:d9e1629da852 16 }
DeguNaoto 52:d9e1629da852 17 }
DeguNaoto 52:d9e1629da852 18 else if(b==6){ /*start*/
DeguNaoto 52:d9e1629da852 19 targ_velocity=speed;
DeguNaoto 52:d9e1629da852 20 sendData(4,defoR);
DeguNaoto 52:d9e1629da852 21 wait(0.1);
DeguNaoto 52:d9e1629da852 22 sendData(5,defoL);
DeguNaoto 52:d9e1629da852 23 wait(0.1);
DeguNaoto 52:d9e1629da852 24 stateR = defoR;
DeguNaoto 52:d9e1629da852 25 stateL = defoL;
DeguNaoto 52:d9e1629da852 26 }
DeguNaoto 52:d9e1629da852 27 else if(b==5){ /*reset*/
DeguNaoto 52:d9e1629da852 28 sendData(4,31);
DeguNaoto 52:d9e1629da852 29 wait(0.1);
DeguNaoto 52:d9e1629da852 30 sendData(5,31);
DeguNaoto 52:d9e1629da852 31 wait(0.1);
DeguNaoto 52:d9e1629da852 32 }
DeguNaoto 52:d9e1629da852 33 else if(b==8){ /*set defo*/
DeguNaoto 52:d9e1629da852 34 sendData(4,defoR);
DeguNaoto 52:d9e1629da852 35 wait(0.1);
DeguNaoto 52:d9e1629da852 36 sendData(5,defoL);
DeguNaoto 52:d9e1629da852 37 wait(0.1);
DeguNaoto 52:d9e1629da852 38 }
DeguNaoto 52:d9e1629da852 39 else if(b==9){ /*stop*/
DeguNaoto 29:03bb1ddbe456 40 Motor_swing=0;
DeguNaoto 22:3996c3f41922 41 sita=PI/4.0,x=0.0,y=0.0;
DeguNaoto 22:3996c3f41922 42 targ_velocity=0.0;
DeguNaoto 22:3996c3f41922 43 targ_sita=PI/4.0;
DeguNaoto 22:3996c3f41922 44 velocity_controller.reset();
DeguNaoto 22:3996c3f41922 45 direction_controller.reset();
DeguNaoto 52:d9e1629da852 46 }
DeguNaoto 52:d9e1629da852 47 else if(b==1){ /*L down*/
DeguNaoto 52:d9e1629da852 48 if(edge1){
DeguNaoto 52:d9e1629da852 49 edge1=0;
DeguNaoto 52:d9e1629da852 50 if(stateL!=1) stateL--;
DeguNaoto 52:d9e1629da852 51 sendData(5,stateL);
DeguNaoto 52:d9e1629da852 52 }
DeguNaoto 52:d9e1629da852 53 }
DeguNaoto 52:d9e1629da852 54 else if(b==2){ /*L up*/
DeguNaoto 52:d9e1629da852 55 if(edge2){
DeguNaoto 52:d9e1629da852 56 edge2=0;
DeguNaoto 52:d9e1629da852 57 if(stateL!=28) stateL++;
DeguNaoto 52:d9e1629da852 58 sendData(5,stateL);
DeguNaoto 22:3996c3f41922 59 }
DeguNaoto 52:d9e1629da852 60 }
DeguNaoto 52:d9e1629da852 61 else if(b==3){ /*R down*/
DeguNaoto 52:d9e1629da852 62 if(edge3){
DeguNaoto 52:d9e1629da852 63 edge3=0;
DeguNaoto 52:d9e1629da852 64 if(stateR!=1) stateR--;
DeguNaoto 52:d9e1629da852 65 sendData(4,stateR);
DeguNaoto 52:d9e1629da852 66 }
DeguNaoto 52:d9e1629da852 67 }
DeguNaoto 52:d9e1629da852 68 else if(b==4){ /*R up*/
DeguNaoto 52:d9e1629da852 69 if(edge4){
DeguNaoto 52:d9e1629da852 70 edge4=0;
DeguNaoto 52:d9e1629da852 71 if(stateR!=28) stateR++;
DeguNaoto 52:d9e1629da852 72 sendData(4,stateR);
DeguNaoto 52:d9e1629da852 73 }
DeguNaoto 52:d9e1629da852 74 }
DeguNaoto 52:d9e1629da852 75 if(b!=7) edge7=1;
DeguNaoto 52:d9e1629da852 76 if(b!=1) edge1=1;
DeguNaoto 52:d9e1629da852 77 if(b!=2) edge2=1;
DeguNaoto 52:d9e1629da852 78 if(b!=3) edge3=1;
DeguNaoto 52:d9e1629da852 79 if(b!=4) edge4=1;
DeguNaoto 22:3996c3f41922 80 }
DeguNaoto 22:3996c3f41922 81
DeguNaoto 21:79b94cb922f0 82 /***The function is PID controller initialize.***/
DeguNaoto 21:79b94cb922f0 83 inline void initializeControllers()
DeguNaoto 21:79b94cb922f0 84 {
DeguNaoto 21:79b94cb922f0 85 velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 21:79b94cb922f0 86 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 21:79b94cb922f0 87
DeguNaoto 21:79b94cb922f0 88 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 21:79b94cb922f0 89 velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 21:79b94cb922f0 90 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 31:74e77ef0831c 91
DeguNaoto 21:79b94cb922f0 92 //set bias. 初期値
DeguNaoto 21:79b94cb922f0 93 velocity_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 94 direction_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 95
DeguNaoto 21:79b94cb922f0 96 //set mode.
DeguNaoto 21:79b94cb922f0 97 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 98 direction_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 99 }
DeguNaoto 21:79b94cb922f0 100
DeguNaoto 21:79b94cb922f0 101 /***The function is following move speed.***/
DeguNaoto 21:79b94cb922f0 102 inline void velocity_following()
DeguNaoto 21:79b94cb922f0 103 {
DeguNaoto 21:79b94cb922f0 104 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 21:79b94cb922f0 105 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 21:79b94cb922f0 106 x1 = (double)velocity_controller.compute();
DeguNaoto 21:79b94cb922f0 107 }
DeguNaoto 21:79b94cb922f0 108
DeguNaoto 21:79b94cb922f0 109 inline void sita_following()
DeguNaoto 21:79b94cb922f0 110 {
DeguNaoto 21:79b94cb922f0 111 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 21:79b94cb922f0 112 direction_controller.setProcessValue((float)sita);
DeguNaoto 55:db1797ac6cb1 113 //direction_controller.setSetPoint(0.0); /*目標値とのずれをなくす*/
DeguNaoto 21:79b94cb922f0 114 //direction_controller.setProcessValue(-y);
DeguNaoto 21:79b94cb922f0 115 x2 = (double)direction_controller.compute();
DeguNaoto 21:79b94cb922f0 116 }
DeguNaoto 21:79b94cb922f0 117
DeguNaoto 21:79b94cb922f0 118 inline void move_following()
DeguNaoto 21:79b94cb922f0 119 {
DeguNaoto 21:79b94cb922f0 120 velocity_following();
DeguNaoto 21:79b94cb922f0 121 sita_following();
DeguNaoto 21:79b94cb922f0 122 Vr = ( x1 + x2 ) / 2.0;
DeguNaoto 21:79b94cb922f0 123 Vl = ( x1 - x2 ) / 2.0;
DeguNaoto 21:79b94cb922f0 124 if( abs(Vr) < 0.05 ) Vr = 0.0;
DeguNaoto 21:79b94cb922f0 125 if( abs(Vl) < 0.05 ) Vl = 0.0;
DeguNaoto 21:79b94cb922f0 126 Move_r( ( float ) Vr );
DeguNaoto 21:79b94cb922f0 127 Move_l( ( float ) Vl );
DeguNaoto 21:79b94cb922f0 128 }
DeguNaoto 21:79b94cb922f0 129
DeguNaoto 21:79b94cb922f0 130 #endif /*autoMode.h*/