ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 25:d44610851105
- Parent:
- 24:e220fbb70ded
- Child:
- 26:7f50323c0c0d
diff -r e220fbb70ded -r d44610851105 quadcopter.cpp --- a/quadcopter.cpp Wed Apr 13 00:57:15 2016 +0000 +++ b/quadcopter.cpp Wed Apr 13 19:16:03 2016 +0000 @@ -90,7 +90,7 @@ // TODO print values to check what they are // TODO convert to Radians (*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); state_.phi = orientation_.roll * M_PI / 180; state_.theta = orientation_.pitch * M_PI / 180; state_.psi = orientation_.heading * M_PI / 180; @@ -184,11 +184,13 @@ pc_->printf("Receive failure\r\n"); } - // TODO convert to radians (starting point: full joystick deflection = +-10° (converted to radian). - - desiredState_.phi=roll-0.5; - desiredState_.theta=pitch-0.5; - desiredState_.psi=yaw-0.5; - F_des_=thrust-0.5; + // convert to radians, range is = +-40° or +-0.698132 radians + desiredState_.phi = ((roll - 0.5) * 80) * M_PI / 180; + desiredState_.theta = ((pitch - 0.5) * 80) * M_PI / 180; + desiredState_.psi = ((yaw - 0.5) * 80) * M_PI / 180; + F_des_ = ((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); }