NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated KANI2017v2

NHK2017Ateamかにろぼ

Dependencies:   2017NHKpin_config mbed FEP HMC6352 MotorDriverController PID QEI omni

classDiagram

　   ＼￣＼　　　　　　　　　　　　　    　 ／￣／ 
/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?

UserRevisionLine numberNew 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 }