Tony YI
/
ESDC2014
123123123123123123123123123
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?
User | Revision | Line number | New 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 | } |