ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 28:61f7356325c3
- Parent:
- 27:11116aa69f32
- Child:
- 29:ae765492fa8b
--- a/quadcopter.cpp Thu Apr 14 22:32:30 2016 +0000 +++ b/quadcopter.cpp Fri Apr 15 19:08:56 2016 +0000 @@ -83,10 +83,6 @@ orientation_.heading -= initial_offsets_->heading; // measured values (will come from IMU/parameter class/Input to function later) - // angles - state_.phi = orientation_.roll; - state_.theta =orientation_.pitch; - state_.psi =orientation_.heading; // angular velocities in body coordinate system state_.p = gyro_event_.gyro.x; state_.q = gyro_event_.gyro.y; @@ -97,9 +93,9 @@ // pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f \r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r); state_.phi = orientation_.roll * M_PI / 180; - state_.theta = orientation_.pitch * M_PI / 180; + state_.theta = -orientation_.pitch * M_PI / 180; state_.psi = orientation_.heading * M_PI / 180; - //pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r); + pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r); } // Date member function @@ -131,9 +127,9 @@ // set pwm values // make code faster by precomputing all the components that are used multiple times and hardcode 0.25/gamma... motorPwm_.m1=zeroVelPwm + 0.25*controlInput_.f-0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz; - motorPwm_.m2=zeroVelPwm + 0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz; + motorPwm_.m2=zeroVelPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz; motorPwm_.m3=zeroVelPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz; - motorPwm_.m4=zeroVelPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz; + motorPwm_.m4=zeroVelPwm + 0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz; motorPwm_.m1 = min(maxPwm, motorPwm_.m1); motorPwm_.m2 = min(maxPwm, motorPwm_.m2); @@ -206,13 +202,15 @@ pc_->printf("Receive failure\r\n"); } + //pc_->printf("buffer: %s\r\n", rxBuffer ); + // convert to radians, range is = +-40° or +-0.698132 radians - desiredState_.phi = ((roll - 0.5) * 80) * M_PI / 180; + desiredState_.phi = -((roll - 0.5) * 80) * M_PI / 180; // minus, because joystick to right should result in positive moment desiredState_.theta = ((pitch - 0.5) * 80) * M_PI / 180; desiredState_.r = yaw-0.5; // number between 0 and 1 //((yaw - 0.5) * 80) * M_PI / 180; F_des_ = thrust-0.5; // number between 0 and 1 //((thrust - 0.5) * 80) * M_PI / 180; // print id with thrust, yaw, pitch, and roll - // pc_->printf("%lld: thrust: %f, yaw: %f, pitch: %f, roll: %f\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi); + //pc_->printf("%lld: thrust: %f, yaw: %f, pitch: %f, roll: %f\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi); }