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@1:3ac2087996f3, 2015-10-28 (annotated)

Committer:
DeguNaoto
Date:
Wed Oct 28 09:26:31 2015 +0000
Revision:
1:3ac2087996f3
Parent:
0:b613dc16f27d
Child:
2:738b28f6a04b
spcount?swing.h????

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 * Functions prototype.
DeguNaoto 0:b613dc16f27d 11 */
DeguNaoto 0:b613dc16f27d 12
DeguNaoto 0:b613dc16f27d 13
DeguNaoto 0:b613dc16f27d 14 /**
DeguNaoto 0:b613dc16f27d 15 * Includes
DeguNaoto 0:b613dc16f27d 16 */
DeguNaoto 0:b613dc16f27d 17 #include "mbed.h"
DeguNaoto 0:b613dc16f27d 18 #include <stdlib.h>
DeguNaoto 0:b613dc16f27d 19 #include <string.h>
DeguNaoto 0:b613dc16f27d 20 #include "QEI.h"
DeguNaoto 0:b613dc16f27d 21 #include "PID.h"
DeguNaoto 0:b613dc16f27d 22
DeguNaoto 0:b613dc16f27d 23 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 24 #include "PinDefinedSettingBlue.h"
DeguNaoto 0:b613dc16f27d 25 #else
DeguNaoto 0:b613dc16f27d 26 #include "PinDefinedSettingRed.h"
DeguNaoto 0:b613dc16f27d 27 #endif
DeguNaoto 0:b613dc16f27d 28
DeguNaoto 0:b613dc16f27d 29 #include "prototype.h"
DeguNaoto 0:b613dc16f27d 30 #include "communicate.h"
DeguNaoto 0:b613dc16f27d 31 #include "Parameters_ps3.h"
DeguNaoto 0:b613dc16f27d 32 #include "manualMode.h"
DeguNaoto 0:b613dc16f27d 33 #include "autoMode.h"
DeguNaoto 0:b613dc16f27d 34
DeguNaoto 0:b613dc16f27d 35 /**
DeguNaoto 0:b613dc16f27d 36 * Functions.
DeguNaoto 0:b613dc16f27d 37 */
DeguNaoto 0:b613dc16f27d 38
DeguNaoto 0:b613dc16f27d 39 /***The function is motors initialize.***/
DeguNaoto 0:b613dc16f27d 40 inline void initializeMotors()
DeguNaoto 0:b613dc16f27d 41 {
DeguNaoto 0:b613dc16f27d 42 Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
DeguNaoto 0:b613dc16f27d 43 Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
DeguNaoto 0:b613dc16f27d 44 }
DeguNaoto 0:b613dc16f27d 45
DeguNaoto 0:b613dc16f27d 46 /***The function is Move on field.***/
DeguNaoto 0:b613dc16f27d 47 //Right
DeguNaoto 0:b613dc16f27d 48 void Move_r(float speed1)
DeguNaoto 0:b613dc16f27d 49 {
DeguNaoto 0:b613dc16f27d 50 if(speed1<0) {
DeguNaoto 0:b613dc16f27d 51 Move_r_Motor_CCW = 1;
DeguNaoto 0:b613dc16f27d 52 Move_r_Motor_CW = 0;
DeguNaoto 0:b613dc16f27d 53 Move_r_Motor_PWM = -speed1;
DeguNaoto 0:b613dc16f27d 54 } else {
DeguNaoto 0:b613dc16f27d 55 Move_r_Motor_CCW = 0;
DeguNaoto 0:b613dc16f27d 56 Move_r_Motor_CW = 1;
DeguNaoto 0:b613dc16f27d 57 Move_r_Motor_PWM = speed1;
DeguNaoto 0:b613dc16f27d 58 }
DeguNaoto 0:b613dc16f27d 59 }
DeguNaoto 0:b613dc16f27d 60 //Left
DeguNaoto 0:b613dc16f27d 61 void Move_l(float speed2)
DeguNaoto 0:b613dc16f27d 62 {
DeguNaoto 0:b613dc16f27d 63 if(speed2<0) {
DeguNaoto 0:b613dc16f27d 64 Move_l_Motor_CCW = 1;
DeguNaoto 0:b613dc16f27d 65 Move_l_Motor_CW = 0;
DeguNaoto 0:b613dc16f27d 66 Move_l_Motor_PWM = -speed2;
DeguNaoto 0:b613dc16f27d 67 } else {
DeguNaoto 0:b613dc16f27d 68 Move_l_Motor_CCW = 0;
DeguNaoto 0:b613dc16f27d 69 Move_l_Motor_CW = 1;
DeguNaoto 0:b613dc16f27d 70 Move_l_Motor_PWM = speed2;
DeguNaoto 0:b613dc16f27d 71 }
DeguNaoto 0:b613dc16f27d 72 }
DeguNaoto 0:b613dc16f27d 73
DeguNaoto 0:b613dc16f27d 74 /***Caluculate state.***/
DeguNaoto 0:b613dc16f27d 75 inline void mesure_state()
DeguNaoto 0:b613dc16f27d 76 {
DeguNaoto 0:b613dc16f27d 77 Pulses_move_r = ( double )Move_r_sense.getPulses();
DeguNaoto 0:b613dc16f27d 78 Pulses_move_l = ( double )Move_l_sense.getPulses();
DeguNaoto 0:b613dc16f27d 79 dlr = ( ( ( Pulses_move_r - PrefPulses_move_r ) / ppr ) * 2.0 * PI ) * r_wheel;
DeguNaoto 0:b613dc16f27d 80 dll = ( ( ( Pulses_move_l - PrefPulses_move_l ) / ppr ) * 2.0 * PI ) * r_wheel;
DeguNaoto 0:b613dc16f27d 81 PrefPulses_move_r = Pulses_move_r;
DeguNaoto 0:b613dc16f27d 82 PrefPulses_move_l = Pulses_move_l;
DeguNaoto 0:b613dc16f27d 83 vr = dlr / RATE;
DeguNaoto 0:b613dc16f27d 84 vl = dll / RATE;
DeguNaoto 0:b613dc16f27d 85 if(flagf) velocity = ( vr + vl ) / 2.0;
DeguNaoto 0:b613dc16f27d 86 else velocity = -( vr + vl ) / 2.0;
DeguNaoto 0:b613dc16f27d 87 dsita = ( dlr - dll ) / ( 2.0 * d );
DeguNaoto 0:b613dc16f27d 88 dx = ( ( dlr + dll ) / 2.0 ) * cos( sita + dsita / 2.0 );
DeguNaoto 0:b613dc16f27d 89 dy = ( ( dlr + dll ) / 2.0 ) * sin( sita + dsita / 2.0 );
DeguNaoto 0:b613dc16f27d 90 sita += dsita;
DeguNaoto 0:b613dc16f27d 91 x += dx;
DeguNaoto 0:b613dc16f27d 92 y += dy;
DeguNaoto 0:b613dc16f27d 93 }
DeguNaoto 0:b613dc16f27d 94
DeguNaoto 0:b613dc16f27d 95 void resetState(){
DeguNaoto 0:b613dc16f27d 96 wait(0.1);
DeguNaoto 0:b613dc16f27d 97 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 98 x=0.0;
DeguNaoto 0:b613dc16f27d 99 y=0.0;
DeguNaoto 0:b613dc16f27d 100 sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 101 targ_sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 102 #else
DeguNaoto 0:b613dc16f27d 103 x=0.0;
DeguNaoto 0:b613dc16f27d 104 y=0.0;
DeguNaoto 0:b613dc16f27d 105 sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 106 targ_sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 107 #endif
DeguNaoto 0:b613dc16f27d 108 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 109 Move_r_sense.reset();
DeguNaoto 0:b613dc16f27d 110 Move_l_sense.reset();
DeguNaoto 0:b613dc16f27d 111 velocity_controller.reset();
DeguNaoto 0:b613dc16f27d 112 direction_controller.reset();
DeguNaoto 0:b613dc16f27d 113 }
DeguNaoto 0:b613dc16f27d 114
DeguNaoto 0:b613dc16f27d 115 #endif /*machine.h*/