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@40:aed70f4dcd06, 2015-11-17 (annotated)

Committer:
DeguNaoto
Date:
Tue Nov 17 07:50:45 2015 +0000
Revision:
40:aed70f4dcd06
Parent:
24:6d2573d6f2b6 
20151117 ?????????

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 24:6d2573d6f2b6 5 PID contSwing(5.917 ,30.0 ,0.0 ,RATE);
DeguNaoto 40:aed70f4dcd06 6 //PID contSwing(6.0 ,100.0 ,0.0 ,RATE);
DeguNaoto 0:b613dc16f27d 7 Ticker interruptSwingSpeed;
DeguNaoto 0:b613dc16f27d 8
DeguNaoto 0:b613dc16f27d 9 int resetSwingSpeed = 0;
DeguNaoto 0:b613dc16f27d 10
DeguNaoto 0:b613dc16f27d 11 inline void countSwingSpeed() {
DeguNaoto 24:6d2573d6f2b6 12 static double i = 0.0;
DeguNaoto 0:b613dc16f27d 13 if (resetSwingSpeed == 1) {
DeguNaoto 24:6d2573d6f2b6 14 i = 0.0;
DeguNaoto 0:b613dc16f27d 15 resetSwingSpeed = 0;
DeguNaoto 0:b613dc16f27d 16 }
DeguNaoto 0:b613dc16f27d 17 if (i <= swingspeed) {
DeguNaoto 0:b613dc16f27d 18 i+=swingspeed/10.0;
DeguNaoto 0:b613dc16f27d 19 targSwingRadVelocity=i;
DeguNaoto 0:b613dc16f27d 20 }
DeguNaoto 0:b613dc16f27d 21 else {
DeguNaoto 24:6d2573d6f2b6 22 interruptSwingSpeed.detach();
DeguNaoto 0:b613dc16f27d 23 }
DeguNaoto 0:b613dc16f27d 24 }
DeguNaoto 0:b613dc16f27d 25
DeguNaoto 0:b613dc16f27d 26
DeguNaoto 0:b613dc16f27d 27 inline void initializeSwing() {
DeguNaoto 0:b613dc16f27d 28 Motor_swing.period_us(SWING_PERIOD);
DeguNaoto 0:b613dc16f27d 29 //controller set
DeguNaoto 24:6d2573d6f2b6 30 contSwing.setInputLimits(0.0, 50.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 7:4aa57a35ec0b 35
DeguNaoto 0:b613dc16f27d 36 double swingSita=0.0;
DeguNaoto 24:6d2573d6f2b6 37 double Rad=0.0;
DeguNaoto 0:b613dc16f27d 38 inline void mesureSwing() {
DeguNaoto 0:b613dc16f27d 39 PulsesSwing = ( double )SwingSens.getPulses();
DeguNaoto 24:6d2573d6f2b6 40 swingRadVelocity = (((PulsesSwing - PrefPulsesSwing) / 1445.0) * 2.0 * PI) / RATE;
DeguNaoto 24:6d2573d6f2b6 41 Rad = (PulsesSwing / 1445.0) * 2.0 * PI;
DeguNaoto 24:6d2573d6f2b6 42 if(Rad>2*PI) Rad-=2*Rad;
DeguNaoto 0:b613dc16f27d 43 PrefPulsesSwing = PulsesSwing;
DeguNaoto 0:b613dc16f27d 44 }
DeguNaoto 0:b613dc16f27d 45
DeguNaoto 24:6d2573d6f2b6 46 float cont=0.0;
DeguNaoto 0:b613dc16f27d 47 inline void swingFollowing(){
DeguNaoto 0:b613dc16f27d 48 contSwing.setSetPoint(( float )targSwingRadVelocity);
DeguNaoto 0:b613dc16f27d 49 contSwing.setProcessValue(( float )swingRadVelocity);
DeguNaoto 0:b613dc16f27d 50 cont = contSwing.compute();
DeguNaoto 0:b613dc16f27d 51 Motor_swing = cont;
DeguNaoto 0:b613dc16f27d 52 }
DeguNaoto 0:b613dc16f27d 53
DeguNaoto 0:b613dc16f27d 54 #endif /*Swing.h*/