camera_platform.cpp

00001 /******************************************************
00002 
00003 ****┏┓          ┏┓
00004 **┏┛┻━━━━━━┛┻┓
00005 **┃                 ┃
00006 **┃      ━━━      ┃
00007 **┃  ┳┛       ┗┳ ┃
00008 **┃                 ┃
00009 **┃ '''    ┻   ''' ┃
00010 **┃                 ┃
00011 **┗━━┓       ┏━━┛
00012 *******┃       ┃
00013 *******┃       ┃
00014 *******┃       ┃
00015 *******┃       ┗━━━━━━━━┓
00016 *******┃                      ┃━┓
00017 *******┃      NO BUG          ┏━┛
00018 *******┃                      ┃
00019 *******┗━┓  ┓  ┏━┏━┓  ━┛
00020 ***********┃  ┛  ┛    ┃  ┛  ┛
00021 ***********┃  ┃  ┃    ┃  ┃  ┃
00022 ***********┗━┛━┛     ┗━┛━┛
00023 
00024 This part is added by project ESDC2014 of CUHK team.
00025 All the code with this header are under GPL open source license.
00026 This program is running on Mbed Platform 'mbed LPC1768' avaliable in 'http://mbed.org'.
00027 **********************************************************/
00028 #include "camera_platform.h"
00029 
00030 //public
00031 Camera_platform::Camera_platform(MyPwmOut* _pwmRoll, MyPwmOut* _pwmPitch, MyPwmOut* _pwmYaw)
00032 {
00033     this->_pwmRoll = _pwmRoll;
00034     this->_pwmPitch = _pwmPitch;
00035     this->_pwmYaw = _pwmYaw;
00036     
00037     _roll_angle = _pitch_angle = _yaw_angle = 0;
00038 }
00039 
00040 Camera_platform::~Camera_platform()
00041 {
00042     delete _pwmRoll;
00043     delete _pwmPitch;
00044     delete _pwmYaw;
00045 }
00046 
00047 void Camera_platform::cameraPlatformMove(uint16_t move_dis, uint8_t move_dir, uint16_t rotate_dis, uint8_t rotate_dir)
00048 {
00049     float _degree = rotate_dis / 100;
00050     
00051     if((rotate_dir & 0xc0) == 0xc0 && (rotate_dir & 0x20) == 0x20) //roll
00052     {
00053         if((rotate_dir & 0x10) == 0) //roll left
00054         {
00055             setRollLeft(_degree);
00056         }
00057         else //roll right
00058         {
00059             setRollRight(_degree);
00060         }
00061     }
00062     if((rotate_dir & 0xc0) == 0xc0 && (rotate_dir & 0x08) == 0x08) //pitch
00063     {
00064         if((rotate_dir & 0x04) == 0) //pitch down
00065         {
00066             setPitchDown(_degree);
00067         }
00068         else //pitch up
00069         {
00070             setPitchUp(_degree);
00071         }
00072     }
00073     if((rotate_dir & 0xc0) == 0xc0 && (rotate_dir & 0x02) == 0x02) //yaw
00074     {
00075         if((rotate_dir & 0x01) == 0) //yaw counter clockwise
00076         {
00077             setYawCClock(_degree);
00078         }
00079         else //yaw right
00080         {
00081             setYawClock(_degree);
00082         }
00083     }
00084 }  
00085 void Camera_platform::setRollLeft(float _degree)
00086 {
00087     setPWM(computePwmValue(_degree, 0, ROLL), ROLL);
00088 }
00089 void Camera_platform::setRollRight(float _degree)
00090 {
00091     setPWM(computePwmValue(_degree, 1, ROLL), ROLL);
00092 }
00093 void Camera_platform::setPitchUp(float _degree)
00094 {
00095     setPWM(computePwmValue(_degree, 1, PITCH), PITCH);
00096 }
00097 void Camera_platform::setPitchDown(float _degree)
00098 {
00099     setPWM(computePwmValue(_degree, 0, PITCH), PITCH);
00100 }
00101 void Camera_platform::setYawClock(float _degree)
00102 {
00103     dir = 1;
00104     setPWM(computePwmValue(_degree, 1, YAW), YAW);
00105 }
00106 void Camera_platform::setYawCClock(float _degree)
00107 {
00108     dir = 0;
00109     setPWM(computePwmValue(_degree, 0, YAW), YAW);
00110 }
00111 void Camera_platform::resetCameraPlatform()
00112 {
00113     setPWM(ROLL_MID, ROLL);
00114     setPWM(PITCH_MID, PITCH);
00115     setPWM(YAW_MID, YAW);
00116 }
00117 
00118 //private
00119 void Camera_platform::setPWM(uint16_t _pwm_value_us, uint8_t _pwm_channel) //0 is roll, 1 is pitch, 2 is yaw
00120 {
00121     switch(_pwm_channel)
00122     {
00123         case ROLL:
00124         _pwmRoll->pulsewidth_us(_pwm_value_us);
00125         break;
00126         case PITCH:
00127         _pwmPitch->pulsewidth_us(_pwm_value_us);
00128         break;
00129         case YAW:
00130         _pwmYaw->pulsewidth_us(_pwm_value_us);
00131         break;
00132         default:
00133         break;
00134     }
00135     wait_ms(800);
00136 }
00137 uint16_t Camera_platform::computePwmValue(float _degree, uint8_t _dir, uint8_t _pwm_channel) //0 is left/up/clock, 1 is right/down/cclock
00138 {
00139     uint16_t return_value = 0;
00140     
00141     switch(_pwm_channel)
00142     {
00143         case ROLL:
00144         if(_dir == 0)
00145         {
00146             _roll_angle -= _degree;
00147             if(_roll_angle < ROLL_ANGLE_MIN)
00148             {
00149                 _roll_angle = ROLL_ANGLE_MIN;
00150             }
00151         }
00152         else if(_dir == 1)
00153         {
00154             _roll_angle += _degree;
00155             if(_roll_angle > ROLL_ANGLE_MAX)
00156             {
00157                 _roll_angle = ROLL_ANGLE_MAX;
00158             }
00159         }
00160         return_value = (uint16_t)(ROLL_MID + _roll_angle * ROLL_USPD);
00161         break;
00162         case PITCH:
00163         if(_dir == 0)
00164         {
00165             _pitch_angle -= _degree;
00166             if(_pitch_angle < PITCH_ANGLE_MIN)
00167             {
00168                 _pitch_angle = PITCH_ANGLE_MIN;
00169             }
00170         }
00171         else if(_dir == 1)
00172         {
00173             _pitch_angle += _degree;
00174             if(_pitch_angle > PITCH_ANGLE_MAX)
00175             {
00176                 _pitch_angle = PITCH_ANGLE_MAX;
00177             }
00178         }
00179         return_value = (uint16_t)(PITCH_MID + _pitch_angle * PITCH_USPD);
00180         break;
00181         case YAW:
00182         if(_dir == 0)
00183         {
00184             _yaw_angle += _degree;
00185             if(_yaw_angle > YAW_ANGLE_MAX)
00186             {
00187                 _yaw_angle = YAW_ANGLE_MAX;
00188             }
00189         }
00190         else if(_dir == 1)
00191         {
00192             _yaw_angle -= _degree;
00193             if(_yaw_angle < YAW_ANGLE_MIN)
00194             {
00195                 _yaw_angle = YAW_ANGLE_MIN;
00196             }
00197         }
00198         
00199         return_value = (uint16_t)(YAW_MID + _yaw_angle * YAW_USPD);
00200         break;
00201         default:
00202         break;
00203     }
00204     
00205     return return_value;
00206 }