File content as of revision 1:3122058b3a2e:

#include "Robot.h"

#define ROBOT_RADIUS (37.0f) // 로봇 회전 반경
#define WHEEL_RADIUS (17.0f) // 바퀴 반지름
#define CONST_DIST (10 / WHEEL_RADIUS * 500 / 3.14159) // 거리 계산 상수

#define STEPS_PER_REV (1000.0f)     // 모터 회전당 기본 스텝수
#define MICRO_STEP_DIV (1)           // 마이크로 스텝수
#define DESIRED_STEPS_PER_SEC (1000) // 모터의 최대 속도 step / sec
#define DESIRED_MICRO_STEPS_PER_SEC (MICRO_STEP_DIV*DESIRED_STEPS_PER_SEC) // 모터의 최대 마이크로 스텝 속도 step / sec
#define MICRO_STEP_PER_REV (STEPS_PER_REV * MICRO_STEP_DIV)// 회전당 총 마이크로스텝 
#define CONST_ANGLE (ROBOT_RADIUS / WHEEL_RADIUS * MICRO_STEP_PER_REV / 360.0f) // 각도 계산 상수

Robot::Robot(PinName l_dir, PinName l_step, PinName _l_enb, PinName r_dir, PinName r_step, PinName _r_enb)
      : LM(mStepper::DRIVER, l_step, l_dir), RM(mStepper::DRIVER, r_step, r_dir),
        l_enb(_l_enb, 0), r_enb(_r_enb, 0) {
    LM.setMaxSpeed(DESIRED_MICRO_STEPS_PER_SEC); // step/sec
    LM.setAcceleration(DESIRED_MICRO_STEPS_PER_SEC * 2);// constant speed within 1s
    RM.setMaxSpeed(DESIRED_MICRO_STEPS_PER_SEC); // step/sec
    RM.setAcceleration(DESIRED_MICRO_STEPS_PER_SEC * 2);// constant speed within 1s
}

void Robot::enable(uint8_t _dir){
    if (_dir == left) l_enb = 1;
    else if (_dir == right) r_enb = 1;
    else if (_dir == both) l_enb = r_enb = 1;
}

void Robot::disable(uint8_t _dir){
    if (_dir == left) l_enb = 0;
    else if (_dir == right) r_enb = 0;
    else if (_dir == both) l_enb = r_enb = 0;
}

uint8_t Robot::isRun(){
    if (LM.distanceToGo() && RM.distanceToGo()) return 3;
    else if (LM.distanceToGo()) return 1;
    else if (RM.distanceToGo()) return 2;
    else return 0;
}

void Robot::run(uint8_t _dir){
    enable(_dir);
    if (_dir == left)LM.run();
    else if (_dir == right) RM.run();
    else if (_dir == both){
        LM.run();
        RM.run();
    }
}

void Robot::run_speed(uint8_t _dir){
    enable(_dir);
    if (_dir == left)LM.runSpeed();
    else if (_dir == right) RM.runSpeed();
    else if (_dir == both){
        LM.runSpeed();
        RM.runSpeed();
    }
}
    

void Robot::stop(uint8_t _dir){
    if (_dir == left) LM.stop();
    else if (_dir == right) RM.stop();
    else if (_dir == both){
        LM.stop();
        RM.stop();
    }
    disable(_dir);
}

void Robot::move_cm(uint8_t _dir, float _distance_cm ) {
    uint32_t step = _distance_cm * CONST_DIST;
    if (_dir == go){
        LM.move(step);
        RM.move(step);
    }
    else if (_dir == back){
        LM.move(-step);
        RM.move(-step);
    }
}

void Robot::move_speed(uint8_t _dir, float _vel){
    LM.setSpeed(DESIRED_MICRO_STEPS_PER_SEC * (_vel / 100));
    RM.setSpeed(DESIRED_MICRO_STEPS_PER_SEC * (_vel / 100));
}

void Robot::turn_deg(uint8_t _dir, float _ang) {
    uint32_t step = _ang * CONST_ANGLE;
    if (_dir == left){
        LM.move(step);
        RM.move(-step);
    }
    else if (_dir == right){
        LM.move(-step);
        RM.move(step);
    }
}

void Robot::turn_LM(uint8_t _dir, float _ang, float _vel = 100){
    uint32_t step = DESIRED_MICRO_STEPS_PER_SEC * (_ang / 360);
    if (_dir == CW) LM.move(step);
    else if (_dir == CCW) LM.move(-step);
}

void Robot::turn_RM(uint8_t _dir, float _ang, float _vel = 100){
    uint32_t step = DESIRED_MICRO_STEPS_PER_SEC * (_ang / 360);
    if(_dir == CW) RM.move(-step);
    else if (_dir == CCW) RM.move(step);
}