Diff: quadcopter.cpp

Revision:

14:64b06476d943

Parent:

13:291ba30c7806

Child:

15:90e07946186f

--- a/quadcopter.cpp	Sat Apr 02 22:06:46 2016 +0000
+++ b/quadcopter.cpp	Thu Apr 07 02:07:33 2016 +0000
@@ -1,12 +1,16 @@
 #include "quadcopter.h"
 #include "sensor.h"
-#include "Adafruit_9DOF.h"
-#include "Serial_base.h"
+#include "receiver.h"
+#include <string>
 
+//#include "mbed.h"
 
 // constructor
-Quadcopter::Quadcopter()
-{
+Quadcopter::Quadcopter(Serial *pcPntr)
+{   
+    pc_= pcPntr;  // enable printing
+    // initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMU
+
     m_=1;
     g_=9.81;
     // proportional attitude control gains
@@ -26,12 +30,18 @@
     accel_ = Adafruit_LSM303_Accel_Unified(30301);
     mag_ = Adafruit_LSM303_Mag_Unified(30302);
     gyro_ = Adafruit_L3GD20_Unified(20);
+    //motor1_(p21);
+    
+    // initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMU
+    
+    // prepare for communication with remote control   
+    MRF24J40 mrf_var(p11, p12, p13, p14, p21);
+    mrf_ = &mrf_var;  // RF tranceiver to link with handheld.
+    rcIterations_ = 100;
+    rcLength_ = 50;
+    rcChannel_ = 3;
 }
 
-void Quadcopter::initAllSensors()
-{
-    initSensors(accel_, mag_, gyro_,offsetAngRate_);  // IMU
-}
 
 void Quadcopter::readSensorValues()
 {
@@ -73,11 +83,6 @@
 
 }
 
-void Quadcopter::print(char * myString)
-{
-    pcPntr_->printf(myString);
-    pcPntr_->printf("\n\r");
-}
 
 void Quadcopter::controller()
 {
@@ -90,18 +95,71 @@
     controlInput_.mx = kp_phi_*(desiredState_.phi-state_.phi)+kd_phi_*(desiredState_.p-state_.p);
     controlInput_.my = kp_theta_*(desiredState_.theta-state_.theta)+kd_theta_*(desiredState_.q-state_.q);
     controlInput_.mz = kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
-    print("Calculated Control");
+    //print("Calculated Control");
 
     //print("F: %f M_x: %f M_y: %f M_z: %f\n\r",  controlInput_.f, controlInput_.mz, controlInput_.my, controlInput_.mz);
-    //        pc.printf("F: %f\n\r",  F);
+    //        pc_->printf("F: %f\n\r",  F);
 }
 
 motors Quadcopter::getPwm()
 {
     //motors motorPwm_;// weired errror, should not be necessary
     motorPwm_.m1=0;
-    motorPwm_.m2=0.65+controlInput_.mx/100;
+    motorPwm_.m2=0.5 + controlInput_.mx/100;
     motorPwm_.m3=0;
-    motorPwm_.m4=0.65-controlInput_.mx/100;
+    motorPwm_.m4=0.5 - controlInput_.mx/100;
     return motorPwm_;
 }
+
+    
+void Quadcopter::readRc() {
+    
+    //write to class variables additional thrust double F_des_, desired state: phi, theta, psi.
+    
+    // RF tranceiver to link with handheld.
+
+    // read from joystick: 
+    
+    //write to class variables additional thrust double F_des_, desired state: phi, theta, psi. 
+    
+    uint8_t zero = 0;
+    uint8_t *rssi = &zero;
+
+    uint8_t receive = 0;
+    int result = 0;
+
+    //Set the Channel. 0 is default, 15 is max
+    mrf_->SetChannel(rcChannel_);
+
+    //Start the timer
+    rcTimer_.start();
+    
+    pc_->printf("START\r\n");
+    
+    // TODO: change default value?
+    float thrust = 0.5;
+    float yaw = 0.5;
+    float pitch = 0.5;
+    float roll = 0.5;
+
+    char rxBuffer[rcLength_];
+    
+    // TODO: stop doing iterations, just do while recieving from slave
+    for (int i = 0; i < rcIterations_; i++) {
+        receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1);
+        if (receive > 0) {
+            pc_->printf("%s \r\n", rxBuffer);
+            result = sscanf(rxBuffer, "%f,%f,%f,%f", &thrust, &yaw, &pitch, &roll);
+            if (result != 4) {
+                pc_->printf("Parse failure\r\n");
+            } else {
+                pc_->printf("%f,%f,%f,%f\r\n", thrust, yaw, pitch, roll);
+            }
+        } else {
+            pc_->printf("Receive failure\r\n");
+        }
+        wait(0.2);  // TODO: change this value?
+    }
+     pc_->printf("END\r\n");    
+}
+