NHK2017Ateamかにろぼ
Dependencies: 2017NHKpin_config mbed FEP HMC6352 MotorDriverController PID QEI omni
\ ̄\ / ̄/ /l \ \ / / lヽ | ヽ ヽ | | / / | \ ` ‐ヽ ヽ ● ● / / ‐ / \ __ l | ||___|| / l __ / \ \ / \/ /\| 人__人 |/\ //\| |/\\ //\| |/\\ / . \_____/ \ ┏┓ ┏━┓┏┓ ┏┓ ┏┓┏┓ ┏┓ ┏┓┗┛ ┏┓ ┗┓┃┗┛ ┏┛┗━┓ ┃┃┃┃ ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ ┗┓┏━┛ ┃┃┗┛ ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛ ┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ ┏┛┃┃┃ ┃┃ ┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ ┗━┛┃┃ ┃┃┏┓ ┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃ ┏┛┃ ┃┃┃┗━━┓ ┗┛┗━━┛┗━━━┛┗┛┗━━┛ ┗━┛ ┗┛┗━━━┛
bot/motor_driver/motor_driver.cpp@23:98746361f827, 2017-09-05 (annotated)
- Committer:
- uchitake
- Date:
- Tue Sep 05 12:02:47 2017 +0900
- Revision:
- 23:98746361f827
- Parent:
- 22:bb6afe7332c3
remove WheelUnit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 6:590c9622ecf1 | 1 | #include "motor_driver.h" |
UCHITAKE | 6:590c9622ecf1 | 2 | |
uchitake |
22:bb6afe7332c3 | 3 | MotorDriver::MotorDriver() : |
uchitake |
22:bb6afe7332c3 | 4 | MDC(MDSDA, MDSCL), |
uchitake |
22:bb6afe7332c3 | 5 | quadOmni(WHEEL_NUMBER), |
uchitake |
22:bb6afe7332c3 | 6 | solenoid(solenoidPin) |
UCHITAKE | 6:590c9622ecf1 | 7 | { |
uchitake |
13:0908c3707020 | 8 | MDC::MotorSTOP(); |
uchitake |
22:bb6afe7332c3 | 9 | quadOmni.setWheelRadian( |
uchitake |
22:bb6afe7332c3 | 10 | WHEEL_RADIAN / 2.0, |
uchitake |
22:bb6afe7332c3 | 11 | WHEEL_RADIAN /2.0 + WHEEL_RADIAN, |
uchitake |
22:bb6afe7332c3 | 12 | -WHEEL_RADIAN / 2.0, |
uchitake |
22:bb6afe7332c3 | 13 | -WHEEL_RADIAN /2.0 - WHEEL_RADIAN |
uchitake |
22:bb6afe7332c3 | 14 | ); |
uchitake |
8:3987aa3d016b | 15 | solenoid = 0; |
UCHITAKE | 6:590c9622ecf1 | 16 | } |
UCHITAKE | 6:590c9622ecf1 | 17 | |
uchitake |
17:6f52f4b1086a | 18 | MotorDriver::MotorDriver(PinName sda, PinName scl, PinName solPin) : |
uchitake |
22:bb6afe7332c3 | 19 | MDC(sda, scl), |
uchitake |
22:bb6afe7332c3 | 20 | quadOmni(WHEEL_NUMBER), |
uchitake |
22:bb6afe7332c3 | 21 | solenoid(solPin) |
uchitake |
17:6f52f4b1086a | 22 | { |
uchitake |
17:6f52f4b1086a | 23 | MDC::MotorSTOP(); |
uchitake |
22:bb6afe7332c3 | 24 | quadOmni.setWheelRadian( |
uchitake |
22:bb6afe7332c3 | 25 | WHEEL_RADIAN / 2.0, |
uchitake |
22:bb6afe7332c3 | 26 | WHEEL_RADIAN /2.0 + WHEEL_RADIAN, |
uchitake |
22:bb6afe7332c3 | 27 | -WHEEL_RADIAN / 2.0, |
uchitake |
22:bb6afe7332c3 | 28 | -WHEEL_RADIAN /2.0 - WHEEL_RADIAN |
uchitake |
22:bb6afe7332c3 | 29 | ); |
uchitake |
17:6f52f4b1086a | 30 | solenoid = 0; |
uchitake |
17:6f52f4b1086a | 31 | } |
UCHITAKE | 19:3a62cbc6fee9 | 32 | |
UCHITAKE | 6:590c9622ecf1 | 33 | void MotorDriver::moveSlider(float speed) |
UCHITAKE | 6:590c9622ecf1 | 34 | { |
uchitake |
13:0908c3707020 | 35 | if(!arm.isResetted() && speed < 0) speed = 0; |
uchitake |
13:0908c3707020 | 36 | if(arm.isPushed() && speed > 0) speed = 0; |
uchitake |
14:a2d77d866d42 | 37 | MDC::write(ARM_MDC_ADDR, SLIDER_NUMBER, speed); |
UCHITAKE | 6:590c9622ecf1 | 38 | } |
UCHITAKE | 6:590c9622ecf1 | 39 | |
uchitake |
8:3987aa3d016b | 40 | void MotorDriver::swing(float speed) |
uchitake |
8:3987aa3d016b | 41 | { |
uchitake |
14:a2d77d866d42 | 42 | MDC::write(ARM_MDC_ADDR, SWORD_NUMBER, speed); |
uchitake |
8:3987aa3d016b | 43 | } |
uchitake |
8:3987aa3d016b | 44 | |
uchitake |
8:3987aa3d016b | 45 | void MotorDriver::shakeHead(float speed) |
uchitake |
8:3987aa3d016b | 46 | { |
uchitake |
14:a2d77d866d42 | 47 | MDC::write(ARM_MDC_ADDR, SHAKE_NUMBER, speed); |
uchitake |
8:3987aa3d016b | 48 | } |
uchitake |
8:3987aa3d016b | 49 | |
uchitake |
8:3987aa3d016b | 50 | void MotorDriver::release() |
uchitake |
8:3987aa3d016b | 51 | { |
uchitake |
8:3987aa3d016b | 52 | solenoid = 1; |
uchitake |
8:3987aa3d016b | 53 | } |
uchitake |
8:3987aa3d016b | 54 | |
UCHITAKE | 6:590c9622ecf1 | 55 | void MotorDriver::destroy(float speed) |
UCHITAKE | 6:590c9622ecf1 | 56 | { |
UCHITAKE | 6:590c9622ecf1 | 57 | write(ARM_MDC_ADDR, ARM_NUMBER, speed); |
UCHITAKE | 6:590c9622ecf1 | 58 | } |
UCHITAKE | 6:590c9622ecf1 | 59 | |
UCHITAKE | 6:590c9622ecf1 | 60 | void MotorDriver::goXY(float X, float Y, float moment) |
UCHITAKE | 6:590c9622ecf1 | 61 | { |
UCHITAKE | 6:590c9622ecf1 | 62 | quadOmni.computeXY(X, Y, moment); |
UCHITAKE | 6:590c9622ecf1 | 63 | for(int i = 0; i < 4; i++) { |
UCHITAKE | 6:590c9622ecf1 | 64 | write(WHEEL_MDC_ADDR, i, quadOmni.getOutput(i)); |
UCHITAKE | 6:590c9622ecf1 | 65 | } |
UCHITAKE | 6:590c9622ecf1 | 66 | } |
UCHITAKE | 6:590c9622ecf1 | 67 | |
UCHITAKE | 6:590c9622ecf1 | 68 | void MotorDriver::goCircular(float r, float rad, float moment) |
UCHITAKE | 6:590c9622ecf1 | 69 | { |
UCHITAKE | 6:590c9622ecf1 | 70 | quadOmni.computeCircular(r, rad, moment); |
UCHITAKE | 6:590c9622ecf1 | 71 | for(int i = 0; i < 4; i++) { |
UCHITAKE | 6:590c9622ecf1 | 72 | write(WHEEL_MDC_ADDR, i, quadOmni.getOutput(i)); |
UCHITAKE | 6:590c9622ecf1 | 73 | } |
uchitake |
22:bb6afe7332c3 | 74 | } |