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@26:760f1bce8214, 2015-11-08 (annotated)

Committer:
DeguNaoto
Date:
Sun Nov 08 10:56:27 2015 +0000
Revision:
26:760f1bce8214
Parent:
9:f9a0c7ca640f
Child:
30:cd344beb415d
220151108 ???????????

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 0:b613dc16f27d 165 void resetState(){
DeguNaoto 0:b613dc16f27d 166 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 167 x=0.0;
DeguNaoto 0:b613dc16f27d 168 y=0.0;
DeguNaoto 0:b613dc16f27d 169 sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 170 targ_sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 171 #else
DeguNaoto 0:b613dc16f27d 172 x=0.0;
DeguNaoto 0:b613dc16f27d 173 y=0.0;
DeguNaoto 0:b613dc16f27d 174 sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 175 targ_sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 176 #endif
DeguNaoto 0:b613dc16f27d 177 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 178 Move_r_sense.reset();
DeguNaoto 0:b613dc16f27d 179 Move_l_sense.reset();
DeguNaoto 0:b613dc16f27d 180 velocity_controller.reset();
DeguNaoto 0:b613dc16f27d 181 direction_controller.reset();
DeguNaoto 0:b613dc16f27d 182 }
DeguNaoto 0:b613dc16f27d 183
DeguNaoto 0:b613dc16f27d 184 #endif /*machine.h*/