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

machine_ps3.h@36:b8954b13a6d5, 2015-11-15 (annotated)

Committer:
DeguNaoto
Date:
Sun Nov 15 00:30:44 2015 +0000
Revision:
36:b8954b13a6d5
Parent:
35:7b6786193aa2
Child:
37:75fcd28f48c7
20151115 Own&middle??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:b613dc16f27d 1 #ifndef MACHINE_H
DeguNaoto 0:b613dc16f27d 2 #define MACHINE_H
DeguNaoto 0:b613dc16f27d 3
DeguNaoto 0:b613dc16f27d 4 /**
DeguNaoto 0:b613dc16f27d 5 * Includes
DeguNaoto 0:b613dc16f27d 6 */
DeguNaoto 0:b613dc16f27d 7 #include "mbed.h"
DeguNaoto 0:b613dc16f27d 8 #include <stdlib.h>
DeguNaoto 0:b613dc16f27d 9 #include <string.h>
DeguNaoto 0:b613dc16f27d 10 #include "QEI.h"
DeguNaoto 0:b613dc16f27d 11 #include "PID.h"
DeguNaoto 9:f9a0c7ca640f 12 #include "PinDefinedSetting.h"
DeguNaoto 0:b613dc16f27d 13 #include "prototype.h"
DeguNaoto 0:b613dc16f27d 14 #include "communicate.h"
DeguNaoto 0:b613dc16f27d 15 #include "Parameters_ps3.h"
DeguNaoto 2:738b28f6a04b 16 #include "Swing.h"
DeguNaoto 6:ca3a74a93ae2 17 #include "autoMode.h"
DeguNaoto 0:b613dc16f27d 18 #include "manualMode.h"
DeguNaoto 5:0e6dd1ff3ff2 19
DeguNaoto 0:b613dc16f27d 20 /**
DeguNaoto 0:b613dc16f27d 21 * Functions.
DeguNaoto 0:b613dc16f27d 22 */
DeguNaoto 26:760f1bce8214 23 /***called by Com***/
DeguNaoto 26:760f1bce8214 24 void Call(){
DeguNaoto 26:760f1bce8214 25 mesure_state();
DeguNaoto 26:760f1bce8214 26 move_following();
DeguNaoto 26:760f1bce8214 27 if(spcount<speed){
DeguNaoto 26:760f1bce8214 28 spcount+=speed/100.0;
DeguNaoto 26:760f1bce8214 29 targ_velocity=spcount;
DeguNaoto 26:760f1bce8214 30 }
DeguNaoto 26:760f1bce8214 31 if(dpcount>0.0){
DeguNaoto 26:760f1bce8214 32 dpcount-=speed/100.0;
DeguNaoto 26:760f1bce8214 33 targ_velocity=dpcount;
DeguNaoto 26:760f1bce8214 34 }
DeguNaoto 26:760f1bce8214 35 mesureSwing();
DeguNaoto 26:760f1bce8214 36 mesure_state();
DeguNaoto 26:760f1bce8214 37 if(enableSwing) swingFollowing();
DeguNaoto 26:760f1bce8214 38 }
DeguNaoto 0:b613dc16f27d 39
DeguNaoto 0:b613dc16f27d 40 /***The function is motors initialize.***/
DeguNaoto 0:b613dc16f27d 41 inline void initializeMotors()
DeguNaoto 0:b613dc16f27d 42 {
DeguNaoto 0:b613dc16f27d 43 Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
DeguNaoto 0:b613dc16f27d 44 Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
DeguNaoto 0:b613dc16f27d 45 }
DeguNaoto 0:b613dc16f27d 46
DeguNaoto 0:b613dc16f27d 47 /***The function is Move on field.***/
DeguNaoto 0:b613dc16f27d 48 //Right
DeguNaoto 0:b613dc16f27d 49 void Move_r(float speed1)
DeguNaoto 0:b613dc16f27d 50 {
DeguNaoto 36:b8954b13a6d5 51 if((speed1<0.2)&&(speed1>-0.2)) speed1=0.0;
DeguNaoto 0:b613dc16f27d 52 if(speed1<0) {
DeguNaoto 0:b613dc16f27d 53 Move_r_Motor_CCW = 1;
DeguNaoto 0:b613dc16f27d 54 Move_r_Motor_CW = 0;
DeguNaoto 0:b613dc16f27d 55 Move_r_Motor_PWM = -speed1;
DeguNaoto 0:b613dc16f27d 56 } else {
DeguNaoto 0:b613dc16f27d 57 Move_r_Motor_CCW = 0;
DeguNaoto 0:b613dc16f27d 58 Move_r_Motor_CW = 1;
DeguNaoto 0:b613dc16f27d 59 Move_r_Motor_PWM = speed1;
DeguNaoto 0:b613dc16f27d 60 }
DeguNaoto 0:b613dc16f27d 61 }
DeguNaoto 0:b613dc16f27d 62 //Left
DeguNaoto 0:b613dc16f27d 63 void Move_l(float speed2)
DeguNaoto 0:b613dc16f27d 64 {
DeguNaoto 36:b8954b13a6d5 65 if((speed2<0.2)&&(speed2>-0.2)) speed2=0.0;
DeguNaoto 0:b613dc16f27d 66 if(speed2<0) {
DeguNaoto 0:b613dc16f27d 67 Move_l_Motor_CCW = 1;
DeguNaoto 0:b613dc16f27d 68 Move_l_Motor_CW = 0;
DeguNaoto 0:b613dc16f27d 69 Move_l_Motor_PWM = -speed2;
DeguNaoto 0:b613dc16f27d 70 } else {
DeguNaoto 0:b613dc16f27d 71 Move_l_Motor_CCW = 0;
DeguNaoto 0:b613dc16f27d 72 Move_l_Motor_CW = 1;
DeguNaoto 0:b613dc16f27d 73 Move_l_Motor_PWM = speed2;
DeguNaoto 0:b613dc16f27d 74 }
DeguNaoto 0:b613dc16f27d 75 }
DeguNaoto 0:b613dc16f27d 76
DeguNaoto 0:b613dc16f27d 77 /***Caluculate state.***/
DeguNaoto 0:b613dc16f27d 78 inline void mesure_state()
DeguNaoto 0:b613dc16f27d 79 {
DeguNaoto 0:b613dc16f27d 80 Pulses_move_r = ( double )Move_r_sense.getPulses();
DeguNaoto 0:b613dc16f27d 81 Pulses_move_l = ( double )Move_l_sense.getPulses();
DeguNaoto 0:b613dc16f27d 82 dlr = ( ( ( Pulses_move_r - PrefPulses_move_r ) / ppr ) * 2.0 * PI ) * r_wheel;
DeguNaoto 0:b613dc16f27d 83 dll = ( ( ( Pulses_move_l - PrefPulses_move_l ) / ppr ) * 2.0 * PI ) * r_wheel;
DeguNaoto 0:b613dc16f27d 84 PrefPulses_move_r = Pulses_move_r;
DeguNaoto 0:b613dc16f27d 85 PrefPulses_move_l = Pulses_move_l;
DeguNaoto 0:b613dc16f27d 86 vr = dlr / RATE;
DeguNaoto 0:b613dc16f27d 87 vl = dll / RATE;
DeguNaoto 0:b613dc16f27d 88 if(flagf) velocity = ( vr + vl ) / 2.0;
DeguNaoto 0:b613dc16f27d 89 else velocity = -( vr + vl ) / 2.0;
DeguNaoto 0:b613dc16f27d 90 dsita = ( dlr - dll ) / ( 2.0 * d );
DeguNaoto 0:b613dc16f27d 91 dx = ( ( dlr + dll ) / 2.0 ) * cos( sita + dsita / 2.0 );
DeguNaoto 0:b613dc16f27d 92 dy = ( ( dlr + dll ) / 2.0 ) * sin( sita + dsita / 2.0 );
DeguNaoto 0:b613dc16f27d 93 sita += dsita;
DeguNaoto 0:b613dc16f27d 94 x += dx;
DeguNaoto 0:b613dc16f27d 95 y += dy;
DeguNaoto 0:b613dc16f27d 96 }
DeguNaoto 0:b613dc16f27d 97
DeguNaoto 26:760f1bce8214 98 /***The function is PID controller initialize.***/
DeguNaoto 26:760f1bce8214 99 inline void initializeControllers()
DeguNaoto 26:760f1bce8214 100 {
DeguNaoto 26:760f1bce8214 101 velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
DeguNaoto 26:760f1bce8214 102 direction_controller.setInputLimits(-PI, PI); //x2
DeguNaoto 26:760f1bce8214 103
DeguNaoto 26:760f1bce8214 104 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 26:760f1bce8214 105 velocity_controller.setOutputLimits(0.0, 1.0);
DeguNaoto 26:760f1bce8214 106 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 26:760f1bce8214 107
DeguNaoto 26:760f1bce8214 108 //set bias. 初期値
DeguNaoto 26:760f1bce8214 109 velocity_controller.setBias(0.0);
DeguNaoto 26:760f1bce8214 110 direction_controller.setBias(0.0);
DeguNaoto 26:760f1bce8214 111
DeguNaoto 26:760f1bce8214 112 //set mode.
DeguNaoto 26:760f1bce8214 113 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 26:760f1bce8214 114 direction_controller.setMode(AUTO_MODE);
DeguNaoto 26:760f1bce8214 115 }
DeguNaoto 26:760f1bce8214 116
DeguNaoto 26:760f1bce8214 117 /***The function is following move speed.***/
DeguNaoto 26:760f1bce8214 118 inline void velocity_following()
DeguNaoto 26:760f1bce8214 119 {
DeguNaoto 26:760f1bce8214 120 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 26:760f1bce8214 121 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 26:760f1bce8214 122 x1 = (double)velocity_controller.compute();
DeguNaoto 26:760f1bce8214 123 }
DeguNaoto 26:760f1bce8214 124
DeguNaoto 26:760f1bce8214 125 inline void arg_following()
DeguNaoto 26:760f1bce8214 126 {
DeguNaoto 26:760f1bce8214 127 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 26:760f1bce8214 128 direction_controller.setProcessValue((float)sita);
DeguNaoto 26:760f1bce8214 129 x2 = (double)direction_controller.compute();
DeguNaoto 26:760f1bce8214 130 }
DeguNaoto 26:760f1bce8214 131
DeguNaoto 26:760f1bce8214 132 inline void move_following()
DeguNaoto 26:760f1bce8214 133 {
DeguNaoto 26:760f1bce8214 134 velocity_following();
DeguNaoto 26:760f1bce8214 135 arg_following();
DeguNaoto 33:a4323c20494b 136 if(autoflag){
DeguNaoto 33:a4323c20494b 137 if(flagf==0){
DeguNaoto 33:a4323c20494b 138 if(x2>0.0){
DeguNaoto 33:a4323c20494b 139 Vr = -x1 + x2;
DeguNaoto 33:a4323c20494b 140 Vl = -x1;
DeguNaoto 33:a4323c20494b 141 }
DeguNaoto 33:a4323c20494b 142 else{
DeguNaoto 33:a4323c20494b 143 Vr = -x1;
DeguNaoto 33:a4323c20494b 144 Vl = -x1 - x2;
DeguNaoto 33:a4323c20494b 145 }
DeguNaoto 26:760f1bce8214 146 }
DeguNaoto 33:a4323c20494b 147 else if(flagf==1){
DeguNaoto 33:a4323c20494b 148 if(x2>0.0){
DeguNaoto 33:a4323c20494b 149 Vr = x1;
DeguNaoto 33:a4323c20494b 150 Vl = x1 - x2;
DeguNaoto 33:a4323c20494b 151 }
DeguNaoto 33:a4323c20494b 152 else{
DeguNaoto 33:a4323c20494b 153 Vr = x1 + x2;
DeguNaoto 33:a4323c20494b 154 Vl = x1;
DeguNaoto 33:a4323c20494b 155 }
DeguNaoto 26:760f1bce8214 156 }
DeguNaoto 35:7b6786193aa2 157 else if(flagf==3){
DeguNaoto 35:7b6786193aa2 158 Vr = ((-x1) + x2)/2.0;
DeguNaoto 35:7b6786193aa2 159 Vl = ((-x1) - x2)/2.0;
DeguNaoto 35:7b6786193aa2 160 }
DeguNaoto 26:760f1bce8214 161 }
DeguNaoto 33:a4323c20494b 162 if(!autoflag){
DeguNaoto 33:a4323c20494b 163 if(flagf==0){
DeguNaoto 33:a4323c20494b 164 Vr = ((-x1) + x2)/2.0;
DeguNaoto 33:a4323c20494b 165 Vl = ((-x1) - x2)/2.0;
DeguNaoto 26:760f1bce8214 166 }
DeguNaoto 33:a4323c20494b 167 else if(flagf==1){
DeguNaoto 33:a4323c20494b 168 Vr = (x1 + x2)/2.0;
DeguNaoto 33:a4323c20494b 169 Vl = (x1 - x2)/2.0;
DeguNaoto 26:760f1bce8214 170 }
DeguNaoto 26:760f1bce8214 171 }
DeguNaoto 26:760f1bce8214 172 Move_r( ( float ) Vr );
DeguNaoto 26:760f1bce8214 173 Move_l( ( float ) Vl );
DeguNaoto 26:760f1bce8214 174 }
DeguNaoto 26:760f1bce8214 175
DeguNaoto 26:760f1bce8214 176 /***The function reset state.***/
DeguNaoto 33:a4323c20494b 177 void resetState(int d){
DeguNaoto 0:b613dc16f27d 178 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 179 x=0.0;
DeguNaoto 0:b613dc16f27d 180 y=0.0;
DeguNaoto 33:a4323c20494b 181 if(d==1){
DeguNaoto 30:cd344beb415d 182 sita=PI/4.0;
DeguNaoto 30:cd344beb415d 183 targ_sita=PI/4.0;
DeguNaoto 30:cd344beb415d 184 }
DeguNaoto 33:a4323c20494b 185 else if(d==0){
DeguNaoto 32:b8c8ad2eeca7 186 sita=3.0*PI/4.0;
DeguNaoto 32:b8c8ad2eeca7 187 targ_sita=3.0*PI/4.0;
DeguNaoto 30:cd344beb415d 188 }
DeguNaoto 33:a4323c20494b 189 else if(d==2){
DeguNaoto 33:a4323c20494b 190 sita=0.0;
DeguNaoto 33:a4323c20494b 191 targ_sita=0.0;
DeguNaoto 33:a4323c20494b 192 }
DeguNaoto 0:b613dc16f27d 193 #else
DeguNaoto 0:b613dc16f27d 194 x=0.0;
DeguNaoto 0:b613dc16f27d 195 y=0.0;
DeguNaoto 33:a4323c20494b 196 if(d==1){
DeguNaoto 30:cd344beb415d 197 sita=-PI/4.0;
DeguNaoto 30:cd344beb415d 198 targ_sita=-PI/4.0;
DeguNaoto 30:cd344beb415d 199 }
DeguNaoto 33:a4323c20494b 200 else if(d==0){
DeguNaoto 32:b8c8ad2eeca7 201 sita=-3.0*PI/4.0;
DeguNaoto 32:b8c8ad2eeca7 202 targ_sita=-3.0*PI/4.0;
DeguNaoto 30:cd344beb415d 203 }
DeguNaoto 33:a4323c20494b 204 else if(d==2){
DeguNaoto 33:a4323c20494b 205 sita=0.0;
DeguNaoto 33:a4323c20494b 206 targ_sita=0.0;
DeguNaoto 33:a4323c20494b 207 }
DeguNaoto 0:b613dc16f27d 208 #endif
DeguNaoto 0:b613dc16f27d 209 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 210 Move_r_sense.reset();
DeguNaoto 0:b613dc16f27d 211 Move_l_sense.reset();
DeguNaoto 0:b613dc16f27d 212 velocity_controller.reset();
DeguNaoto 0:b613dc16f27d 213 direction_controller.reset();
DeguNaoto 0:b613dc16f27d 214 }
DeguNaoto 0:b613dc16f27d 215
DeguNaoto 30:cd344beb415d 216 #endif /*machine.h*/