Diff: RC_Receiver/ROBOFRIEN_RCV.cpp

Revision:

0:a1ad0eb8b619

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RC_Receiver/ROBOFRIEN_RCV.cpp	Mon Apr 16 07:16:00 2018 +0000
@@ -0,0 +1,114 @@
+#include "mbed.h"
+#include "ROBOFRIEN_RCV.h"
+
+static Timer RCV_Timer;
+
+int16_t rcv_channel[6];
+bool rc_state[8];
+/////// RCV 1 //////////////
+int rcv1_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv1_rise(){
+    rcv1_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv1_fall(){
+    rcv_channel[0] = RCV_Timer.read_us() - rcv1_pulse_time;
+    rc_state[0] = true;
+}
+/////// RCV 2 //////////////
+int rcv2_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv2_rise(){
+    rcv2_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv2_fall(){
+    rcv_channel[1] = RCV_Timer.read_us() - rcv2_pulse_time;
+    rc_state[1] = true;
+}
+/////// RCV 3 //////////////
+int rcv3_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv3_rise(){
+    rcv3_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv3_fall(){
+    rcv_channel[2] = RCV_Timer.read_us() - rcv3_pulse_time;
+    rc_state[2] = true;
+}
+/////// RCV 4 //////////////
+int rcv4_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv4_rise(){
+    rcv4_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv4_fall(){
+    rcv_channel[3] = RCV_Timer.read_us() - rcv4_pulse_time;
+    rc_state[3] = true;
+}
+/////// RCV 5 //////////////
+int rcv5_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv5_rise(){
+    rcv5_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv5_fall(){
+    rcv_channel[4] = RCV_Timer.read_us() - rcv5_pulse_time;
+    rc_state[4] = true;
+}
+/////// RCV 6 //////////////
+int rcv6_pulse_time;
+void ROBOFRIEN_RCV::isr_rc_rcv6_rise(){
+    rcv6_pulse_time = RCV_Timer.read_us();
+}
+void ROBOFRIEN_RCV::isr_rc_rcv6_fall(){
+    rcv_channel[5] = RCV_Timer.read_us() - rcv6_pulse_time;
+    rc_state[5] = true;
+}
+
+ROBOFRIEN_RCV::ROBOFRIEN_RCV(PinName RCV1) : _InterruptIn1(RCV1)
+{
+    if(RCV1 == p5){
+        _InterruptIn1.mode(PullDown);
+        _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv1_rise);
+        _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv1_fall);        
+    }
+    else if(RCV1 == p6){
+        _InterruptIn1.mode(PullDown);
+        _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv2_rise);
+        _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv2_fall);        
+    }
+    else if(RCV1 == p7){
+        _InterruptIn1.mode(PullDown);
+        _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv3_rise);
+        _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv3_fall);        
+    }
+    else if(RCV1 == p8){
+        _InterruptIn1.mode(PullDown);
+        _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv4_rise);
+        _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv4_fall);        
+    }
+    else if(RCV1 == p11){
+        _InterruptIn1.mode(PullDown);
+        _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv5_rise);
+        _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv5_fall);        
+    }
+    else if(RCV1 == p12){
+        _InterruptIn1.mode(PullDown);
+        _InterruptIn1.rise(this,&ROBOFRIEN_RCV::isr_rc_rcv6_rise);
+        _InterruptIn1.fall(this,&ROBOFRIEN_RCV::isr_rc_rcv6_fall);        
+    }
+}
+void ROBOFRIEN_RCV::Init(){
+    RCV_Timer.start();
+}
+
+///////// LPF ////////////////
+void ROBOFRIEN_RCV::Update(){
+    for(int i=0; i<6; i++){
+        if(rc_state[i] == true){
+            rc_state[i] = false;
+            if(i != 2){
+                lpf_cap[i] = lpf_cap[i] * 0.9 + (rcv_channel[i]/10.0) * 0.1;               
+            }else{
+                lpf_cap[i] = lpf_cap[i] * 0.7 + (rcv_channel[i]/10.0) * 0.3;        
+            }            
+        }
+    }
+}
+
+