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

machine_ps3.h@26:8e6c736b6791, 2015-09-18 (annotated)

Committer:
DeguNaoto
Date:
Fri Sep 18 04:00:33 2015 +0000
Revision:
26:8e6c736b6791
Parent:
22:3996c3f41922
Child:
29:03bb1ddbe456
??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:bd4719e15f7e 1 /**
DeguNaoto 2:cf8ca6742db9 2 * This library is included main.cpp.
DeguNaoto 0:bd4719e15f7e 3 * class "machine" whitch machine functions are structured.
DeguNaoto 0:bd4719e15f7e 4 */
DeguNaoto 0:bd4719e15f7e 5
DeguNaoto 0:bd4719e15f7e 6 #ifndef MACHINE_H
DeguNaoto 0:bd4719e15f7e 7 #define MACHINE_H
DeguNaoto 0:bd4719e15f7e 8
DeguNaoto 0:bd4719e15f7e 9 /**
DeguNaoto 21:79b94cb922f0 10 * Functions prototype.
DeguNaoto 21:79b94cb922f0 11 */
DeguNaoto 21:79b94cb922f0 12
DeguNaoto 21:79b94cb922f0 13
DeguNaoto 21:79b94cb922f0 14 /**
DeguNaoto 0:bd4719e15f7e 15 * Includes
DeguNaoto 0:bd4719e15f7e 16 */
DeguNaoto 0:bd4719e15f7e 17 #include "mbed.h"
DeguNaoto 0:bd4719e15f7e 18 #include "QEI.h"
DeguNaoto 0:bd4719e15f7e 19 #include "PID.h"
DeguNaoto 0:bd4719e15f7e 20 #include "PinDefined_and_Setting_ps3.h"
DeguNaoto 21:79b94cb922f0 21 #include "prototype.h"
DeguNaoto 22:3996c3f41922 22 #include "communicate.h"
DeguNaoto 0:bd4719e15f7e 23 #include "Parameters_ps3.h"
DeguNaoto 22:3996c3f41922 24 #include "manualMode.h"
DeguNaoto 21:79b94cb922f0 25 #include "autoMode.h"
DeguNaoto 0:bd4719e15f7e 26
DeguNaoto 0:bd4719e15f7e 27 /**
DeguNaoto 0:bd4719e15f7e 28 * Functions.
DeguNaoto 0:bd4719e15f7e 29 */
DeguNaoto 0:bd4719e15f7e 30
DeguNaoto 0:bd4719e15f7e 31 /***The function is motors initialize.***/
DeguNaoto 2:cf8ca6742db9 32 inline void initializeMotors()
DeguNaoto 2:cf8ca6742db9 33 {
DeguNaoto 0:bd4719e15f7e 34 Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 35 Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 36 }
DeguNaoto 0:bd4719e15f7e 37
DeguNaoto 2:cf8ca6742db9 38 /***The function is Move on field.***/
DeguNaoto 0:bd4719e15f7e 39 //Right
DeguNaoto 2:cf8ca6742db9 40 void Move_r(float speed1)
DeguNaoto 2:cf8ca6742db9 41 {
DeguNaoto 2:cf8ca6742db9 42 if(speed1<0) {
DeguNaoto 21:79b94cb922f0 43 Move_r_Motor_CCW = 1;
DeguNaoto 21:79b94cb922f0 44 Move_r_Motor_CW = 0;
DeguNaoto 21:79b94cb922f0 45 Move_r_Motor_PWM = abs(speed1);
DeguNaoto 2:cf8ca6742db9 46 } else {
DeguNaoto 21:79b94cb922f0 47 Move_r_Motor_CCW = 0;
DeguNaoto 21:79b94cb922f0 48 Move_r_Motor_CW = 1;
DeguNaoto 21:79b94cb922f0 49 Move_r_Motor_PWM = speed1;
DeguNaoto 0:bd4719e15f7e 50 }
DeguNaoto 0:bd4719e15f7e 51 }
DeguNaoto 0:bd4719e15f7e 52 //Left
DeguNaoto 2:cf8ca6742db9 53 void Move_l(float speed2)
DeguNaoto 2:cf8ca6742db9 54 {
DeguNaoto 2:cf8ca6742db9 55 if(speed2<0) {
DeguNaoto 21:79b94cb922f0 56 Move_l_Motor_CCW = 1;
DeguNaoto 21:79b94cb922f0 57 Move_l_Motor_CW = 0;
DeguNaoto 21:79b94cb922f0 58 Move_l_Motor_PWM = abs(speed2);
DeguNaoto 2:cf8ca6742db9 59 } else {
DeguNaoto 21:79b94cb922f0 60 Move_l_Motor_CCW = 0;
DeguNaoto 21:79b94cb922f0 61 Move_l_Motor_CW = 1;
DeguNaoto 21:79b94cb922f0 62 Move_l_Motor_PWM = speed2;
DeguNaoto 0:bd4719e15f7e 63 }
DeguNaoto 0:bd4719e15f7e 64 }
DeguNaoto 0:bd4719e15f7e 65
DeguNaoto 11:ce3083681efa 66 /***Caluculate state.***/
DeguNaoto 11:ce3083681efa 67 inline void mesure_state()
DeguNaoto 2:cf8ca6742db9 68 {
DeguNaoto 19:598211462097 69 Pulses_move_r = ( double )Move_r_sense.getPulses();
DeguNaoto 19:598211462097 70 Pulses_move_l = ( double )Move_l_sense.getPulses();
DeguNaoto 19:598211462097 71 dlr = ( ( ( Pulses_move_r - PrefPulses_move_r ) / ppr ) * 2.0 * PI ) * r_wheel;
DeguNaoto 19:598211462097 72 dll = ( ( ( Pulses_move_l - PrefPulses_move_l ) / ppr ) * 2.0 * PI ) * r_wheel;
DeguNaoto 19:598211462097 73 PrefPulses_move_r = Pulses_move_r;
DeguNaoto 19:598211462097 74 PrefPulses_move_l = Pulses_move_l;
DeguNaoto 19:598211462097 75 vr = dlr / RATE;
DeguNaoto 19:598211462097 76 vl = dll / RATE;
DeguNaoto 19:598211462097 77 velocity = ( vr + vl ) / 2.0;
DeguNaoto 19:598211462097 78 dsita = ( dlr - dll ) / ( 2.0 * d );
DeguNaoto 19:598211462097 79 dx = ( ( dlr + dll ) / 2.0 ) * cos( sita + dsita / 2.0 );
DeguNaoto 19:598211462097 80 dy = ( ( dlr + dll ) / 2.0 ) * sin( sita + dsita / 2.0 );
DeguNaoto 19:598211462097 81 sita += dsita;
DeguNaoto 19:598211462097 82 x += dx;
DeguNaoto 19:598211462097 83 y += dy;
DeguNaoto 0:bd4719e15f7e 84 }
DeguNaoto 11:ce3083681efa 85
DeguNaoto 19:598211462097 86 /***Interrupter to shoot.***/
DeguNaoto 19:598211462097 87 inline void initialize_interrupter() {
DeguNaoto 19:598211462097 88 interrupter.rise(&shootEsaka);
DeguNaoto 19:598211462097 89 }
DeguNaoto 18:526124eef0d1 90
DeguNaoto 19:598211462097 91 void shootEsaka() {
DeguNaoto 19:598211462097 92 Indicator1 = !Indicator1;
DeguNaoto 19:598211462097 93 }
DeguNaoto 18:526124eef0d1 94
DeguNaoto 0:bd4719e15f7e 95 #endif /*machine.h*/