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@114:325e4c158141, 2015-10-27 (annotated)

Committer:
DeguNaoto
Date:
Tue Oct 27 07:49:43 2015 +0000
Revision:
114:325e4c158141
Parent:
107:579bb1ab67d9 
??????????; ????ver

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