Tony YI / Mbed 2 deprecated ESDC2014

123123123123123123123123123

Dependencies:   mbed

camera_platform.cpp@3:4306d042af6f, 2014-07-03 (annotated)

Committer:
TonyYI
Date:
Thu Jul 03 14:52:44 2014 +0000
Revision:
3:4306d042af6f
Parent:
0:3417ca0a36c0 
123123

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TonyYI 0:3417ca0a36c0 1 /******************************************************
TonyYI 0:3417ca0a36c0 2
TonyYI 0:3417ca0a36c0 3 ****┏┓ ┏┓
TonyYI 0:3417ca0a36c0 4 **┏┛┻━━━━━━┛┻┓
TonyYI 0:3417ca0a36c0 5 **┃ ┃
TonyYI 0:3417ca0a36c0 6 **┃ ━━━ ┃
TonyYI 0:3417ca0a36c0 7 **┃ ┳┛ ┗┳ ┃
TonyYI 0:3417ca0a36c0 8 **┃ ┃
TonyYI 0:3417ca0a36c0 9 **┃ ''' ┻ ''' ┃
TonyYI 0:3417ca0a36c0 10 **┃ ┃
TonyYI 0:3417ca0a36c0 11 **┗━━┓ ┏━━┛
TonyYI 0:3417ca0a36c0 12 *******┃ ┃
TonyYI 0:3417ca0a36c0 13 *******┃ ┃
TonyYI 0:3417ca0a36c0 14 *******┃ ┃
TonyYI 0:3417ca0a36c0 15 *******┃ ┗━━━━━━━━┓
TonyYI 0:3417ca0a36c0 16 *******┃ ┃━┓
TonyYI 0:3417ca0a36c0 17 *******┃ NO BUG ┏━┛
TonyYI 0:3417ca0a36c0 18 *******┃ ┃
TonyYI 0:3417ca0a36c0 19 *******┗━┓ ┓ ┏━┏━┓ ━┛
TonyYI 0:3417ca0a36c0 20 ***********┃ ┛ ┛ ┃ ┛ ┛
TonyYI 0:3417ca0a36c0 21 ***********┃ ┃ ┃ ┃ ┃ ┃
TonyYI 0:3417ca0a36c0 22 ***********┗━┛━┛ ┗━┛━┛
TonyYI 0:3417ca0a36c0 23
TonyYI 0:3417ca0a36c0 24 This part is added by project ESDC2014 of CUHK team.
TonyYI 0:3417ca0a36c0 25 All the code with this header are under GPL open source license.
TonyYI 0:3417ca0a36c0 26 This program is running on Mbed Platform 'mbed LPC1768' avaliable in 'http://mbed.org'.
TonyYI 0:3417ca0a36c0 27 **********************************************************/
TonyYI 0:3417ca0a36c0 28 #include "camera_platform.h"
TonyYI 0:3417ca0a36c0 29
TonyYI 0:3417ca0a36c0 30 //public
TonyYI 0:3417ca0a36c0 31 Camera_platform::Camera_platform(MyPwmOut* _pwmRoll, MyPwmOut* _pwmPitch, MyPwmOut* _pwmYaw)
TonyYI 0:3417ca0a36c0 32 {
TonyYI 0:3417ca0a36c0 33 this->_pwmRoll = _pwmRoll;
TonyYI 0:3417ca0a36c0 34 this->_pwmPitch = _pwmPitch;
TonyYI 0:3417ca0a36c0 35 this->_pwmYaw = _pwmYaw;
TonyYI 0:3417ca0a36c0 36
TonyYI 0:3417ca0a36c0 37 _roll_angle = _pitch_angle = _yaw_angle = 0;
TonyYI 0:3417ca0a36c0 38 }
TonyYI 0:3417ca0a36c0 39
TonyYI 0:3417ca0a36c0 40 Camera_platform::~Camera_platform()
TonyYI 0:3417ca0a36c0 41 {
TonyYI 0:3417ca0a36c0 42 delete _pwmRoll;
TonyYI 0:3417ca0a36c0 43 delete _pwmPitch;
TonyYI 0:3417ca0a36c0 44 delete _pwmYaw;
TonyYI 0:3417ca0a36c0 45 }
TonyYI 0:3417ca0a36c0 46
TonyYI 0:3417ca0a36c0 47 void Camera_platform::cameraPlatformMove(uint16_t move_dis, uint8_t move_dir, uint16_t rotate_dis, uint8_t rotate_dir)
TonyYI 0:3417ca0a36c0 48 {
TonyYI 0:3417ca0a36c0 49 float _degree = rotate_dis / 100;
TonyYI 0:3417ca0a36c0 50
TonyYI 0:3417ca0a36c0 51 if((rotate_dir & 0xc0) == 0xc0 && (rotate_dir & 0x20) == 0x20) //roll
TonyYI 0:3417ca0a36c0 52 {
TonyYI 0:3417ca0a36c0 53 if((rotate_dir & 0x10) == 0) //roll left
TonyYI 0:3417ca0a36c0 54 {
TonyYI 0:3417ca0a36c0 55 setRollLeft(_degree);
TonyYI 0:3417ca0a36c0 56 }
TonyYI 0:3417ca0a36c0 57 else //roll right
TonyYI 0:3417ca0a36c0 58 {
TonyYI 0:3417ca0a36c0 59 setRollRight(_degree);
TonyYI 0:3417ca0a36c0 60 }
TonyYI 0:3417ca0a36c0 61 }
TonyYI 0:3417ca0a36c0 62 if((rotate_dir & 0xc0) == 0xc0 && (rotate_dir & 0x08) == 0x08) //pitch
TonyYI 0:3417ca0a36c0 63 {
TonyYI 0:3417ca0a36c0 64 if((rotate_dir & 0x04) == 0) //pitch down
TonyYI 0:3417ca0a36c0 65 {
TonyYI 0:3417ca0a36c0 66 setPitchDown(_degree);
TonyYI 0:3417ca0a36c0 67 }
TonyYI 0:3417ca0a36c0 68 else //pitch up
TonyYI 0:3417ca0a36c0 69 {
TonyYI 0:3417ca0a36c0 70 setPitchUp(_degree);
TonyYI 0:3417ca0a36c0 71 }
TonyYI 0:3417ca0a36c0 72 }
TonyYI 0:3417ca0a36c0 73 if((rotate_dir & 0xc0) == 0xc0 && (rotate_dir & 0x02) == 0x02) //yaw
TonyYI 0:3417ca0a36c0 74 {
TonyYI 0:3417ca0a36c0 75 if((rotate_dir & 0x01) == 0) //yaw counter clockwise
TonyYI 0:3417ca0a36c0 76 {
TonyYI 0:3417ca0a36c0 77 setYawCClock(_degree);
TonyYI 0:3417ca0a36c0 78 }
TonyYI 0:3417ca0a36c0 79 else //yaw right
TonyYI 0:3417ca0a36c0 80 {
TonyYI 0:3417ca0a36c0 81 setYawClock(_degree);
TonyYI 0:3417ca0a36c0 82 }
TonyYI 0:3417ca0a36c0 83 }
TonyYI 0:3417ca0a36c0 84 }
TonyYI 0:3417ca0a36c0 85 void Camera_platform::setRollLeft(float _degree)
TonyYI 0:3417ca0a36c0 86 {
TonyYI 0:3417ca0a36c0 87 setPWM(computePwmValue(_degree, 0, ROLL), ROLL);
TonyYI 0:3417ca0a36c0 88 }
TonyYI 0:3417ca0a36c0 89 void Camera_platform::setRollRight(float _degree)
TonyYI 0:3417ca0a36c0 90 {
TonyYI 0:3417ca0a36c0 91 setPWM(computePwmValue(_degree, 1, ROLL), ROLL);
TonyYI 0:3417ca0a36c0 92 }
TonyYI 0:3417ca0a36c0 93 void Camera_platform::setPitchUp(float _degree)
TonyYI 0:3417ca0a36c0 94 {
TonyYI 0:3417ca0a36c0 95 setPWM(computePwmValue(_degree, 1, PITCH), PITCH);
TonyYI 0:3417ca0a36c0 96 }
TonyYI 0:3417ca0a36c0 97 void Camera_platform::setPitchDown(float _degree)
TonyYI 0:3417ca0a36c0 98 {
TonyYI 0:3417ca0a36c0 99 setPWM(computePwmValue(_degree, 0, PITCH), PITCH);
TonyYI 0:3417ca0a36c0 100 }
TonyYI 0:3417ca0a36c0 101 void Camera_platform::setYawClock(float _degree)
TonyYI 0:3417ca0a36c0 102 {
TonyYI 0:3417ca0a36c0 103 dir = 1;
TonyYI 0:3417ca0a36c0 104 setPWM(computePwmValue(_degree, 1, YAW), YAW);
TonyYI 0:3417ca0a36c0 105 }
TonyYI 0:3417ca0a36c0 106 void Camera_platform::setYawCClock(float _degree)
TonyYI 0:3417ca0a36c0 107 {
TonyYI 0:3417ca0a36c0 108 dir = 0;
TonyYI 0:3417ca0a36c0 109 setPWM(computePwmValue(_degree, 0, YAW), YAW);
TonyYI 0:3417ca0a36c0 110 }
TonyYI 0:3417ca0a36c0 111 void Camera_platform::resetCameraPlatform()
TonyYI 0:3417ca0a36c0 112 {
TonyYI 0:3417ca0a36c0 113 setPWM(ROLL_MID, ROLL);
TonyYI 0:3417ca0a36c0 114 setPWM(PITCH_MID, PITCH);
TonyYI 0:3417ca0a36c0 115 setPWM(YAW_MID, YAW);
TonyYI 0:3417ca0a36c0 116 }
TonyYI 0:3417ca0a36c0 117
TonyYI 0:3417ca0a36c0 118 //private
TonyYI 0:3417ca0a36c0 119 void Camera_platform::setPWM(uint16_t _pwm_value_us, uint8_t _pwm_channel) //0 is roll, 1 is pitch, 2 is yaw
TonyYI 0:3417ca0a36c0 120 {
TonyYI 0:3417ca0a36c0 121 switch(_pwm_channel)
TonyYI 0:3417ca0a36c0 122 {
TonyYI 0:3417ca0a36c0 123 case ROLL:
TonyYI 0:3417ca0a36c0 124 _pwmRoll->pulsewidth_us(_pwm_value_us);
TonyYI 0:3417ca0a36c0 125 break;
TonyYI 0:3417ca0a36c0 126 case PITCH:
TonyYI 0:3417ca0a36c0 127 _pwmPitch->pulsewidth_us(_pwm_value_us);
TonyYI 0:3417ca0a36c0 128 break;
TonyYI 0:3417ca0a36c0 129 case YAW:
TonyYI 0:3417ca0a36c0 130 _pwmYaw->pulsewidth_us(_pwm_value_us);
TonyYI 0:3417ca0a36c0 131 break;
TonyYI 0:3417ca0a36c0 132 default:
TonyYI 0:3417ca0a36c0 133 break;
TonyYI 0:3417ca0a36c0 134 }
TonyYI 0:3417ca0a36c0 135 wait_ms(800);
TonyYI 0:3417ca0a36c0 136 }
TonyYI 0:3417ca0a36c0 137 uint16_t Camera_platform::computePwmValue(float _degree, uint8_t _dir, uint8_t _pwm_channel) //0 is left/up/clock, 1 is right/down/cclock
TonyYI 0:3417ca0a36c0 138 {
TonyYI 0:3417ca0a36c0 139 uint16_t return_value = 0;
TonyYI 0:3417ca0a36c0 140
TonyYI 0:3417ca0a36c0 141 switch(_pwm_channel)
TonyYI 0:3417ca0a36c0 142 {
TonyYI 0:3417ca0a36c0 143 case ROLL:
TonyYI 0:3417ca0a36c0 144 if(_dir == 0)
TonyYI 0:3417ca0a36c0 145 {
TonyYI 0:3417ca0a36c0 146 _roll_angle -= _degree;
TonyYI 0:3417ca0a36c0 147 if(_roll_angle < ROLL_ANGLE_MIN)
TonyYI 0:3417ca0a36c0 148 {
TonyYI 0:3417ca0a36c0 149 _roll_angle = ROLL_ANGLE_MIN;
TonyYI 0:3417ca0a36c0 150 }
TonyYI 0:3417ca0a36c0 151 }
TonyYI 0:3417ca0a36c0 152 else if(_dir == 1)
TonyYI 0:3417ca0a36c0 153 {
TonyYI 0:3417ca0a36c0 154 _roll_angle += _degree;
TonyYI 0:3417ca0a36c0 155 if(_roll_angle > ROLL_ANGLE_MAX)
TonyYI 0:3417ca0a36c0 156 {
TonyYI 0:3417ca0a36c0 157 _roll_angle = ROLL_ANGLE_MAX;
TonyYI 0:3417ca0a36c0 158 }
TonyYI 0:3417ca0a36c0 159 }
TonyYI 0:3417ca0a36c0 160 return_value = (uint16_t)(ROLL_MID + _roll_angle * ROLL_USPD);
TonyYI 0:3417ca0a36c0 161 break;
TonyYI 0:3417ca0a36c0 162 case PITCH:
TonyYI 0:3417ca0a36c0 163 if(_dir == 0)
TonyYI 0:3417ca0a36c0 164 {
TonyYI 0:3417ca0a36c0 165 _pitch_angle -= _degree;
TonyYI 0:3417ca0a36c0 166 if(_pitch_angle < PITCH_ANGLE_MIN)
TonyYI 0:3417ca0a36c0 167 {
TonyYI 0:3417ca0a36c0 168 _pitch_angle = PITCH_ANGLE_MIN;
TonyYI 0:3417ca0a36c0 169 }
TonyYI 0:3417ca0a36c0 170 }
TonyYI 0:3417ca0a36c0 171 else if(_dir == 1)
TonyYI 0:3417ca0a36c0 172 {
TonyYI 0:3417ca0a36c0 173 _pitch_angle += _degree;
TonyYI 0:3417ca0a36c0 174 if(_pitch_angle > PITCH_ANGLE_MAX)
TonyYI 0:3417ca0a36c0 175 {
TonyYI 0:3417ca0a36c0 176 _pitch_angle = PITCH_ANGLE_MAX;
TonyYI 0:3417ca0a36c0 177 }
TonyYI 0:3417ca0a36c0 178 }
TonyYI 0:3417ca0a36c0 179 return_value = (uint16_t)(PITCH_MID + _pitch_angle * PITCH_USPD);
TonyYI 0:3417ca0a36c0 180 break;
TonyYI 0:3417ca0a36c0 181 case YAW:
TonyYI 0:3417ca0a36c0 182 if(_dir == 0)
TonyYI 0:3417ca0a36c0 183 {
TonyYI 0:3417ca0a36c0 184 _yaw_angle += _degree;
TonyYI 0:3417ca0a36c0 185 if(_yaw_angle > YAW_ANGLE_MAX)
TonyYI 0:3417ca0a36c0 186 {
TonyYI 0:3417ca0a36c0 187 _yaw_angle = YAW_ANGLE_MAX;
TonyYI 0:3417ca0a36c0 188 }
TonyYI 0:3417ca0a36c0 189 }
TonyYI 0:3417ca0a36c0 190 else if(_dir == 1)
TonyYI 0:3417ca0a36c0 191 {
TonyYI 0:3417ca0a36c0 192 _yaw_angle -= _degree;
TonyYI 0:3417ca0a36c0 193 if(_yaw_angle < YAW_ANGLE_MIN)
TonyYI 0:3417ca0a36c0 194 {
TonyYI 0:3417ca0a36c0 195 _yaw_angle = YAW_ANGLE_MIN;
TonyYI 0:3417ca0a36c0 196 }
TonyYI 0:3417ca0a36c0 197 }
TonyYI 0:3417ca0a36c0 198
TonyYI 0:3417ca0a36c0 199 return_value = (uint16_t)(YAW_MID + _yaw_angle * YAW_USPD);
TonyYI 0:3417ca0a36c0 200 break;
TonyYI 0:3417ca0a36c0 201 default:
TonyYI 0:3417ca0a36c0 202 break;
TonyYI 0:3417ca0a36c0 203 }
TonyYI 0:3417ca0a36c0 204
TonyYI 0:3417ca0a36c0 205 return return_value;
TonyYI 0:3417ca0a36c0 206 }