kubtss / Mbed 2 deprecated BIRD2017

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:   mbed-rtos mbed

Control/Trim.cpp@3:e3c41153e5fe, 2015-09-29 (annotated)

Committer:
shimogamo
Date:
Tue Sep 29 16:03:01 2015 +0000
Revision:
3:e3c41153e5fe
Parent:
2:e0f1e8662b8c
Child:
8:ca92cb674004
servo??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimogamo 0:2a15bd367891 1 #include "mbed.h"
shimogamo 0:2a15bd367891 2 #include "Trim.h"
shimogamo 0:2a15bd367891 3 #include "Global.h"
shimogamo 0:2a15bd367891 4 //PullUpに注意
shimogamo 0:2a15bd367891 5 Trim::Trim(PinName upsw, PinName downsw, PinName modesw)
shimogamo 0:2a15bd367891 6 : _upsw(upsw), _downsw(downsw), _modesw(modesw){
shimogamo 0:2a15bd367891 7
shimogamo 0:2a15bd367891 8 _upsw.mode(PullUp);
shimogamo 0:2a15bd367891 9 _downsw.mode(PullUp);
shimogamo 0:2a15bd367891 10 _modesw.mode(PullUp);
shimogamo 0:2a15bd367891 11
shimogamo 0:2a15bd367891 12 upswstatus = 1;
shimogamo 0:2a15bd367891 13 downswstatus = 1;
shimogamo 0:2a15bd367891 14
shimogamo 0:2a15bd367891 15 pitchrate = 0.5;
shimogamo 0:2a15bd367891 16 maxmoderate = 1.38;
shimogamo 0:2a15bd367891 17 moderate = -maxmoderate;
shimogamo 0:2a15bd367891 18
shimogamo 0:2a15bd367891 19 _ticker_trim.attach(this,&Trim::modechanger,0.1);//トリムモードの急激変化緩和用の定周期割り込み
shimogamo 0:2a15bd367891 20
shimogamo 0:2a15bd367891 21 }
shimogamo 0:2a15bd367891 22
shimogamo 0:2a15bd367891 23
shimogamo 0:2a15bd367891 24 void Trim::initialize(){
shimogamo 0:2a15bd367891 25 //pitchrateとmaxmoderateの調整
shimogamo 0:2a15bd367891 26 }
shimogamo 0:2a15bd367891 27
shimogamo 0:2a15bd367891 28 void Trim::pitchup(){
shimogamo 0:2a15bd367891 29 trimpitch--;
shimogamo 0:2a15bd367891 30 clamp(trimpitch,-7,7);
shimogamo 0:2a15bd367891 31 printf("trimpitch = %d\n",trimpitch);
shimogamo 0:2a15bd367891 32 }
shimogamo 0:2a15bd367891 33
shimogamo 0:2a15bd367891 34 void Trim::pitchdown(){
shimogamo 0:2a15bd367891 35 trimpitch++;
shimogamo 0:2a15bd367891 36 clamp(trimpitch,-7,7);
shimogamo 0:2a15bd367891 37 printf("trimpitch = %d\n",trimpitch);
shimogamo 0:2a15bd367891 38 }
shimogamo 0:2a15bd367891 39
shimogamo 0:2a15bd367891 40 void Trim::clamp(int &value, int min, int max){
shimogamo 0:2a15bd367891 41 if(value < min) {
shimogamo 0:2a15bd367891 42 value = min;
shimogamo 0:2a15bd367891 43 } else if(value > max) {
shimogamo 0:2a15bd367891 44 value = max;
shimogamo 0:2a15bd367891 45 }
shimogamo 0:2a15bd367891 46 }
shimogamo 0:2a15bd367891 47
shimogamo 0:2a15bd367891 48 void Trim::clamp(double &value, double min, double max){
shimogamo 0:2a15bd367891 49 if(value < min) {
shimogamo 0:2a15bd367891 50 value = min;
shimogamo 0:2a15bd367891 51 } else if(value > max) {
shimogamo 0:2a15bd367891 52 value = max;
shimogamo 0:2a15bd367891 53 }
shimogamo 0:2a15bd367891 54 }
shimogamo 0:2a15bd367891 55
shimogamo 0:2a15bd367891 56
shimogamo 0:2a15bd367891 57 double Trim::calc(int trimpitch, bool trimmode){
shimogamo 0:2a15bd367891 58 return (double)trimpitch*pitchrate + moderate;
shimogamo 0:2a15bd367891 59 }
shimogamo 0:2a15bd367891 60
shimogamo 2:e0f1e8662b8c 61 //16年度は多分使わない
shimogamo 0:2a15bd367891 62 void Trim::modechanger(){
shimogamo 0:2a15bd367891 63 if(_modesw == 0){//moderate急激変化の緩和
shimogamo 0:2a15bd367891 64 moderate += 0.1;//1回あたりの変化量
shimogamo 0:2a15bd367891 65 }else{
shimogamo 0:2a15bd367891 66 moderate -= 0.1;//1回あたりの変化量
shimogamo 0:2a15bd367891 67 }
shimogamo 0:2a15bd367891 68 clamp(moderate, -maxmoderate, 0);
shimogamo 0:2a15bd367891 69 }
shimogamo 0:2a15bd367891 70
shimogamo 0:2a15bd367891 71
shimogamo 0:2a15bd367891 72 void Trim::update(){
shimogamo 0:2a15bd367891 73 if(_upsw == 0){
shimogamo 0:2a15bd367891 74 upswstatus = 0;
shimogamo 0:2a15bd367891 75 }else if((_upsw == 1)&&(upswstatus == 0)){
shimogamo 0:2a15bd367891 76 pitchup();
shimogamo 0:2a15bd367891 77 upswstatus = 1;
shimogamo 0:2a15bd367891 78 }
shimogamo 0:2a15bd367891 79
shimogamo 0:2a15bd367891 80 if(_downsw == 0){
shimogamo 0:2a15bd367891 81 downswstatus = 0;
shimogamo 0:2a15bd367891 82 }else if((_downsw == 1)&&(downswstatus == 0)){
shimogamo 0:2a15bd367891 83 pitchdown();
shimogamo 0:2a15bd367891 84 downswstatus = 1;
shimogamo 0:2a15bd367891 85 }
shimogamo 0:2a15bd367891 86
shimogamo 0:2a15bd367891 87 Global::settrimpitch(calc(trimpitch,_modesw.read()));
shimogamo 0:2a15bd367891 88 Global::setinttrimpitch(trimpitch);
shimogamo 0:2a15bd367891 89
shimogamo 0:2a15bd367891 90 }