Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

Hardwares/Motor.cpp

Committer:
hazheng
Date:
2017-03-29
Revision:
44:15de535c4005
Parent:
13:7dcb1642ef99
Child:
46:a5eb9bd3bb55

File content as of revision 44:15de535c4005:

#include "Motor.h"
#include "mbed.h"

#include "Core.h"
#include "SWUSBServer.h"

#include "PinAssignment.h"

#define MOTOR_PERIOD  0.002

Motor::Motor(SW::Core& core) :
    m_core(core),
    m_dirL(DigitalOut(PIN_MC_DIR_L, MDIR_Forward)),
    m_dirR(DigitalOut(PIN_MC_DIR_R, MDIR_Forward)),
    m_pwmL(PwmOut(PIN_MC_SPEED_L)),
    m_pwmR(PwmOut(PIN_MC_SPEED_R))
    
{
    m_pwmL.period(MOTOR_PERIOD);
    m_pwmR.period(MOTOR_PERIOD);
    m_pwmL = 0.0f;
    m_pwmR = 0.0f;
}


void Motor::Update(float deltaTime)
{
    
}

void Motor::setLeftSpeed(const float speed)
{
    if(speed < -1.0f || speed > 1.0f)
        return;
    
    //m_pwmL.period_ms(MOTOR_PERIOD);
    
    setLeftDirection(speed < 0 ? MDIR_Backward : MDIR_Forward);
    m_pwmL.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
    //m_pwmL = (speed < 0 ? -speed : speed);

    char buf[20];
    sprintf(buf, "L %f-%f", speed, (float)m_pwmL);
    m_core.GetUSBServer().PushUnreliableMsg('D', buf);
    //m_pwmL = speed;
}

void Motor::setRightSpeed(const float speed)
{
    if(speed < -1.0f || speed > 1.0f)
        return;
    
    //m_pwmR.period_ms(MOTOR_PERIOD);
    
    setRightDirection(speed < 0 ? MDIR_Backward : MDIR_Forward);
    m_pwmR.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
    //m_pwmR = (speed < 0 ? -speed : speed);
    
    char buf[20];
    sprintf(buf, "R %f-%f", speed, (float)m_pwmR);
    m_core.GetUSBServer().PushUnreliableMsg('D', buf);

    //m_pwmR = speed;
}    

void Motor::setSpeeds(const float leftSpeed, const float rightSpeed)
{
    setLeftSpeed(leftSpeed);
    setRightSpeed(rightSpeed);    
}

    
void Motor::setLeftDirection(MotorDir dir)
{
    m_dirL = static_cast<int>(dir);
}
    
void Motor::setRightDirection(MotorDir dir)
{
    m_dirR = static_cast<int>(dir);
}
    
void Motor::setDirections(MotorDir dirL, MotorDir dirR)
{
    setLeftDirection(dirL);
    setRightDirection(dirR);
}