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

Swing.h@0:b613dc16f27d, 2015-10-28 (annotated)

Committer:
DeguNaoto
Date:
Wed Oct 28 09:03:19 2015 +0000
Revision:
0:b613dc16f27d
Child:
7:4aa57a35ec0b
20151028 Ticker??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:b613dc16f27d 1 #ifndef SWING_H
DeguNaoto 0:b613dc16f27d 2 #define SWING_H
DeguNaoto 0:b613dc16f27d 3
DeguNaoto 0:b613dc16f27d 4 /***Swing.***/
DeguNaoto 0:b613dc16f27d 5 PID contSwing(20.917 ,2.0 ,0.0 ,RATE);
DeguNaoto 0:b613dc16f27d 6 Ticker interruptSwingSpeed;
DeguNaoto 0:b613dc16f27d 7
DeguNaoto 0:b613dc16f27d 8 int resetSwingSpeed = 0;
DeguNaoto 0:b613dc16f27d 9
DeguNaoto 0:b613dc16f27d 10 inline void countSwingSpeed() {
DeguNaoto 0:b613dc16f27d 11 static int i = 0;
DeguNaoto 0:b613dc16f27d 12 if (resetSwingSpeed == 1) {
DeguNaoto 0:b613dc16f27d 13 i = 0;
DeguNaoto 0:b613dc16f27d 14 resetSwingSpeed = 0;
DeguNaoto 0:b613dc16f27d 15 }
DeguNaoto 0:b613dc16f27d 16 if (i <= swingspeed) {
DeguNaoto 0:b613dc16f27d 17 i+=swingspeed/10.0;
DeguNaoto 0:b613dc16f27d 18 targSwingRadVelocity=i;
DeguNaoto 0:b613dc16f27d 19 }
DeguNaoto 0:b613dc16f27d 20 else {
DeguNaoto 0:b613dc16f27d 21 interruptSwingSpeed.detach();
DeguNaoto 0:b613dc16f27d 22 }
DeguNaoto 0:b613dc16f27d 23 }
DeguNaoto 0:b613dc16f27d 24
DeguNaoto 0:b613dc16f27d 25
DeguNaoto 0:b613dc16f27d 26 inline void initializeSwing() {
DeguNaoto 0:b613dc16f27d 27 interrupter.rise(&countSwing);
DeguNaoto 0:b613dc16f27d 28 Motor_swing.period_us(SWING_PERIOD);
DeguNaoto 0:b613dc16f27d 29 //controller set
DeguNaoto 0:b613dc16f27d 30 contSwing.setInputLimits(0.0, 100.0);
DeguNaoto 0:b613dc16f27d 31 contSwing.setOutputLimits(0.0, 1.0);
DeguNaoto 0:b613dc16f27d 32 contSwing.setBias(0.0);
DeguNaoto 0:b613dc16f27d 33 contSwing.setMode(AUTO_MODE);
DeguNaoto 0:b613dc16f27d 34 }
DeguNaoto 0:b613dc16f27d 35 float waitTime = 0.0;
DeguNaoto 0:b613dc16f27d 36 void countSwing() {
DeguNaoto 0:b613dc16f27d 37 Indicator1 = !Indicator1;
DeguNaoto 0:b613dc16f27d 38 if(enableShoot){
DeguNaoto 0:b613dc16f27d 39 enableShoot=0;
DeguNaoto 0:b613dc16f27d 40 // waitTime = (float)(((2.0 * PI) / swingRadVelocity) / 2.0);
DeguNaoto 0:b613dc16f27d 41 // waitTime = (float)((((10.0 * PI) / 4.0) / swingRadVelocity) / 8.0);
DeguNaoto 0:b613dc16f27d 42 if(swingRadVelocity==0.0) waitTime = (float)((((2.0 * PI) / swingRadVelocity) / 4.0));//-0.0017);
DeguNaoto 0:b613dc16f27d 43 else waitTime=0.0;
DeguNaoto 0:b613dc16f27d 44 if(waitTime<0.0) waitTime=0.0;
DeguNaoto 0:b613dc16f27d 45 if(waitTime>10.0) waitTime=10.0;
DeguNaoto 0:b613dc16f27d 46 wait(waitTime);
DeguNaoto 0:b613dc16f27d 47 sendData(1,6);
DeguNaoto 0:b613dc16f27d 48 }
DeguNaoto 0:b613dc16f27d 49 }
DeguNaoto 0:b613dc16f27d 50
DeguNaoto 0:b613dc16f27d 51 double swingSita=0.0;
DeguNaoto 0:b613dc16f27d 52 inline void mesureSwing() {
DeguNaoto 0:b613dc16f27d 53 PulsesSwing = ( double )SwingSens.getPulses();
DeguNaoto 0:b613dc16f27d 54 swingRadVelocity = (((PulsesSwing - PrefPulsesSwing) / 3000.0) * 2.0 * PI) / RATE;
DeguNaoto 0:b613dc16f27d 55 PrefPulsesSwing = PulsesSwing;
DeguNaoto 0:b613dc16f27d 56 }
DeguNaoto 0:b613dc16f27d 57
DeguNaoto 0:b613dc16f27d 58 double cont=0.0;
DeguNaoto 0:b613dc16f27d 59 inline void swingFollowing(){
DeguNaoto 0:b613dc16f27d 60 contSwing.setSetPoint(( float )targSwingRadVelocity);
DeguNaoto 0:b613dc16f27d 61 contSwing.setProcessValue(( float )swingRadVelocity);
DeguNaoto 0:b613dc16f27d 62 cont = contSwing.compute();
DeguNaoto 0:b613dc16f27d 63 Motor_swing = cont;
DeguNaoto 0:b613dc16f27d 64 }
DeguNaoto 0:b613dc16f27d 65
DeguNaoto 0:b613dc16f27d 66 #endif /*Swing.h*/