HoSeung Choi / step_motor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   ft_robot1

Robot.cpp@1:3122058b3a2e, 2020-08-31 (annotated)

Committer:
d2_h10s
Date:
Mon Aug 31 15:09:14 2020 +0000
Revision:
1:3122058b3a2e
Parent:
0:283fc48ef873 
for braille teams

Who changed what in which revision?

UserRevisionLine numberNew contents of line
d2_h10s 0:283fc48ef873 1 #include "Robot.h"
d2_h10s 0:283fc48ef873 2
d2_h10s 0:283fc48ef873 3 #define ROBOT_RADIUS (37.0f) // 로봇 회전 반경
d2_h10s 0:283fc48ef873 4 #define WHEEL_RADIUS (17.0f) // 바퀴 반지름
d2_h10s 0:283fc48ef873 5 #define CONST_DIST (10 / WHEEL_RADIUS * 500 / 3.14159) // 거리 계산 상수
d2_h10s 0:283fc48ef873 6
d2_h10s 0:283fc48ef873 7 #define STEPS_PER_REV (1000.0f) // 모터 회전당 기본 스텝수
d2_h10s 0:283fc48ef873 8 #define MICRO_STEP_DIV (1) // 마이크로 스텝수
d2_h10s 0:283fc48ef873 9 #define DESIRED_STEPS_PER_SEC (1000) // 모터의 최대 속도 step / sec
d2_h10s 0:283fc48ef873 10 #define DESIRED_MICRO_STEPS_PER_SEC (MICRO_STEP_DIV*DESIRED_STEPS_PER_SEC) // 모터의 최대 마이크로 스텝 속도 step / sec
d2_h10s 0:283fc48ef873 11 #define MICRO_STEP_PER_REV (STEPS_PER_REV * MICRO_STEP_DIV)// 회전당 총 마이크로스텝
d2_h10s 0:283fc48ef873 12 #define CONST_ANGLE (ROBOT_RADIUS / WHEEL_RADIUS * MICRO_STEP_PER_REV / 360.0f) // 각도 계산 상수
d2_h10s 0:283fc48ef873 13
d2_h10s 0:283fc48ef873 14 Robot::Robot(PinName l_dir, PinName l_step, PinName _l_enb, PinName r_dir, PinName r_step, PinName _r_enb)
d2_h10s 0:283fc48ef873 15 : LM(mStepper::DRIVER, l_step, l_dir), RM(mStepper::DRIVER, r_step, r_dir),
d2_h10s 0:283fc48ef873 16 l_enb(_l_enb, 0), r_enb(_r_enb, 0) {
d2_h10s 0:283fc48ef873 17 LM.setMaxSpeed(DESIRED_MICRO_STEPS_PER_SEC); // step/sec
d2_h10s 0:283fc48ef873 18 LM.setAcceleration(DESIRED_MICRO_STEPS_PER_SEC * 2);// constant speed within 1s
d2_h10s 0:283fc48ef873 19 RM.setMaxSpeed(DESIRED_MICRO_STEPS_PER_SEC); // step/sec
d2_h10s 0:283fc48ef873 20 RM.setAcceleration(DESIRED_MICRO_STEPS_PER_SEC * 2);// constant speed within 1s
d2_h10s 0:283fc48ef873 21 }
d2_h10s 0:283fc48ef873 22
d2_h10s 0:283fc48ef873 23 void Robot::enable(uint8_t _dir){
d2_h10s 0:283fc48ef873 24 if (_dir == left) l_enb = 1;
d2_h10s 0:283fc48ef873 25 else if (_dir == right) r_enb = 1;
d2_h10s 0:283fc48ef873 26 else if (_dir == both) l_enb = r_enb = 1;
d2_h10s 0:283fc48ef873 27 }
d2_h10s 0:283fc48ef873 28
d2_h10s 0:283fc48ef873 29 void Robot::disable(uint8_t _dir){
d2_h10s 0:283fc48ef873 30 if (_dir == left) l_enb = 0;
d2_h10s 0:283fc48ef873 31 else if (_dir == right) r_enb = 0;
d2_h10s 0:283fc48ef873 32 else if (_dir == both) l_enb = r_enb = 0;
d2_h10s 0:283fc48ef873 33 }
d2_h10s 0:283fc48ef873 34
d2_h10s 0:283fc48ef873 35 uint8_t Robot::isRun(){
d2_h10s 0:283fc48ef873 36 if (LM.distanceToGo() && RM.distanceToGo()) return 3;
d2_h10s 0:283fc48ef873 37 else if (LM.distanceToGo()) return 1;
d2_h10s 0:283fc48ef873 38 else if (RM.distanceToGo()) return 2;
d2_h10s 0:283fc48ef873 39 else return 0;
d2_h10s 0:283fc48ef873 40 }
d2_h10s 0:283fc48ef873 41
d2_h10s 0:283fc48ef873 42 void Robot::run(uint8_t _dir){
d2_h10s 0:283fc48ef873 43 enable(_dir);
d2_h10s 0:283fc48ef873 44 if (_dir == left)LM.run();
d2_h10s 0:283fc48ef873 45 else if (_dir == right) RM.run();
d2_h10s 0:283fc48ef873 46 else if (_dir == both){
d2_h10s 0:283fc48ef873 47 LM.run();
d2_h10s 0:283fc48ef873 48 RM.run();
d2_h10s 0:283fc48ef873 49 }
d2_h10s 0:283fc48ef873 50 }
d2_h10s 0:283fc48ef873 51
d2_h10s 0:283fc48ef873 52 void Robot::run_speed(uint8_t _dir){
d2_h10s 0:283fc48ef873 53 enable(_dir);
d2_h10s 0:283fc48ef873 54 if (_dir == left)LM.runSpeed();
d2_h10s 0:283fc48ef873 55 else if (_dir == right) RM.runSpeed();
d2_h10s 0:283fc48ef873 56 else if (_dir == both){
d2_h10s 0:283fc48ef873 57 LM.runSpeed();
d2_h10s 0:283fc48ef873 58 RM.runSpeed();
d2_h10s 0:283fc48ef873 59 }
d2_h10s 0:283fc48ef873 60 }
d2_h10s 0:283fc48ef873 61
d2_h10s 0:283fc48ef873 62
d2_h10s 0:283fc48ef873 63 void Robot::stop(uint8_t _dir){
d2_h10s 0:283fc48ef873 64 if (_dir == left) LM.stop();
d2_h10s 0:283fc48ef873 65 else if (_dir == right) RM.stop();
d2_h10s 0:283fc48ef873 66 else if (_dir == both){
d2_h10s 0:283fc48ef873 67 LM.stop();
d2_h10s 0:283fc48ef873 68 RM.stop();
d2_h10s 0:283fc48ef873 69 }
d2_h10s 0:283fc48ef873 70 disable(_dir);
d2_h10s 0:283fc48ef873 71 }
d2_h10s 0:283fc48ef873 72
d2_h10s 0:283fc48ef873 73 void Robot::move_cm(uint8_t _dir, float _distance_cm ) {
d2_h10s 0:283fc48ef873 74 uint32_t step = _distance_cm * CONST_DIST;
d2_h10s 0:283fc48ef873 75 if (_dir == go){
d2_h10s 0:283fc48ef873 76 LM.move(step);
d2_h10s 0:283fc48ef873 77 RM.move(step);
d2_h10s 0:283fc48ef873 78 }
d2_h10s 0:283fc48ef873 79 else if (_dir == back){
d2_h10s 0:283fc48ef873 80 LM.move(-step);
d2_h10s 0:283fc48ef873 81 RM.move(-step);
d2_h10s 0:283fc48ef873 82 }
d2_h10s 0:283fc48ef873 83 }
d2_h10s 0:283fc48ef873 84
d2_h10s 0:283fc48ef873 85 void Robot::move_speed(uint8_t _dir, float _vel){
d2_h10s 1:3122058b3a2e 86 LM.setSpeed(DESIRED_MICRO_STEPS_PER_SEC * (_vel / 100));
d2_h10s 1:3122058b3a2e 87 RM.setSpeed(DESIRED_MICRO_STEPS_PER_SEC * (_vel / 100));
d2_h10s 0:283fc48ef873 88 }
d2_h10s 0:283fc48ef873 89
d2_h10s 0:283fc48ef873 90 void Robot::turn_deg(uint8_t _dir, float _ang) {
d2_h10s 0:283fc48ef873 91 uint32_t step = _ang * CONST_ANGLE;
d2_h10s 0:283fc48ef873 92 if (_dir == left){
d2_h10s 0:283fc48ef873 93 LM.move(step);
d2_h10s 0:283fc48ef873 94 RM.move(-step);
d2_h10s 0:283fc48ef873 95 }
d2_h10s 0:283fc48ef873 96 else if (_dir == right){
d2_h10s 0:283fc48ef873 97 LM.move(-step);
d2_h10s 0:283fc48ef873 98 RM.move(step);
d2_h10s 0:283fc48ef873 99 }
d2_h10s 0:283fc48ef873 100 }
d2_h10s 0:283fc48ef873 101
d2_h10s 0:283fc48ef873 102 void Robot::turn_LM(uint8_t _dir, float _ang, float _vel = 100){
d2_h10s 0:283fc48ef873 103 uint32_t step = DESIRED_MICRO_STEPS_PER_SEC * (_ang / 360);
d2_h10s 0:283fc48ef873 104 if (_dir == CW) LM.move(step);
d2_h10s 0:283fc48ef873 105 else if (_dir == CCW) LM.move(-step);
d2_h10s 0:283fc48ef873 106 }
d2_h10s 0:283fc48ef873 107
d2_h10s 0:283fc48ef873 108 void Robot::turn_RM(uint8_t _dir, float _ang, float _vel = 100){
d2_h10s 0:283fc48ef873 109 uint32_t step = DESIRED_MICRO_STEPS_PER_SEC * (_ang / 360);
d2_h10s 0:283fc48ef873 110 if(_dir == CW) RM.move(-step);
d2_h10s 0:283fc48ef873 111 else if (_dir == CCW) RM.move(step);
d2_h10s 0:283fc48ef873 112 }