ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 14:64b06476d943
- Parent:
- 13:291ba30c7806
- Child:
- 15:90e07946186f
diff -r 291ba30c7806 -r 64b06476d943 quadcopter.cpp --- 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"); +} +