Diff: Control/Trim.cpp

Revision:

0:2a15bd367891

Child:

2:e0f1e8662b8c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/Trim.cpp	Mon Sep 07 01:19:55 2015 +0000
@@ -0,0 +1,92 @@
+#include "mbed.h"
+#include "Trim.h"
+#include "Global.h"
+//PullUpに注意
+Trim::Trim(PinName upsw, PinName downsw, PinName modesw) 
+: _upsw(upsw), _downsw(downsw), _modesw(modesw){
+    
+    _upsw.mode(PullUp);
+    _downsw.mode(PullUp);
+    _modesw.mode(PullUp);
+    
+    upswstatus = 1;
+    downswstatus = 1;
+    
+    pitchrate = 0.5;
+    maxmoderate = 1.38;
+    moderate = -maxmoderate;
+    
+    _ticker_trim.attach(this,&Trim::modechanger,0.1);//トリムモードの急激変化緩和用の定周期割り込み
+
+}
+
+
+void Trim::initialize(){
+    //pitchrateとmaxmoderateの調整
+}
+
+void Trim::pitchup(){
+    trimpitch--;
+    clamp(trimpitch,-7,7);
+    printf("trimpitch = %d\n",trimpitch);
+}
+
+void Trim::pitchdown(){
+    trimpitch++;
+    clamp(trimpitch,-7,7);
+    printf("trimpitch = %d\n",trimpitch);
+}
+
+void Trim::clamp(int &value, int min, int max){
+    if(value < min) {
+        value = min;
+    } else if(value > max) {
+        value = max;
+    }
+}
+
+void Trim::clamp(double &value, double min, double max){
+    if(value < min) {
+        value = min;
+    } else if(value > max) {
+        value = max;
+    }
+}
+
+
+double Trim::calc(int trimpitch, bool trimmode){
+    return (double)trimpitch*pitchrate + moderate;
+}
+
+//今年は多分いらない
+void Trim::modechanger(){
+    if(_modesw == 0){//moderate急激変化の緩和
+        moderate += 0.1;//1回あたりの変化量
+    }else{
+        moderate -= 0.1;//1回あたりの変化量
+    }
+    clamp(moderate, -maxmoderate, 0);
+}
+    
+
+void Trim::update(){
+    if(_upsw == 0){
+        upswstatus = 0;
+    }else if((_upsw == 1)&&(upswstatus == 0)){
+        pitchup();
+        upswstatus = 1;
+    }
+    
+    if(_downsw == 0){
+        downswstatus = 0;
+    }else if((_downsw == 1)&&(downswstatus == 0)){
+        pitchdown();
+        downswstatus = 1;
+    }
+    
+        
+    Global::settrimpitch(calc(trimpitch,_modesw.read()));
+    Global::setinttrimpitch(trimpitch);
+    
+    printf("trim\n");
+}
\ No newline at end of file