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@32:b8c8ad2eeca7, 2015-11-11 (annotated)

Committer:
DeguNaoto
Date:
Wed Nov 11 06:40:39 2015 +0000
Revision:
32:b8c8ad2eeca7
Parent:
30:cd344beb415d
Child:
33:a4323c20494b
20151110 ???

Who changed what in which revision?

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