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-04-06
Revision:
52:078b521c9edf
Parent:
47:a682be9908b9
Child:
87:15fcf7891bf9

File content as of revision 52:078b521c9edf:

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

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

#include "PinAssignment.h"
#include "GlobalVariable.h"

#define MOTOR_PERIOD  0.020f



#ifdef __cplusplus
extern "C" {
#endif

static DigitalOut motor_dir_l(PIN_MC_DIR_L, MDIR_Forward);
static DigitalOut motor_dir_r(PIN_MC_DIR_R, MDIR_Forward);

static PwmOut motor_pwm_l(PIN_MC_SPEED_L);
static PwmOut motor_pwm_r(PIN_MC_SPEED_R);
    
void motor_init()
{
    motor_pwm_l.period(MOTOR_PERIOD);
    motor_pwm_r.period(MOTOR_PERIOD);
    motor_dir_l = 0.0f;
    motor_dir_r = 0.0f;
}

void motor_set_left_speed(const float speed)
{
    if(speed < -1.0f || speed > 1.0f)
        return;
    
    motor_set_left_direction(speed < 0 ? MDIR_Backward : MDIR_Forward);
    
    motor_pwm_l.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD * MOTOR_MAX_SPEED_LIMIT);
    
    //char buf[20];
    //sprintf(buf, "Motor %f", (float)motor_pwm_l);
    //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
}

void motor_set_right_speed(const float speed)
{
    if(speed < -1.0f || speed > 1.0f)
        return;
    
    motor_set_right_direction(speed < 0 ? MDIR_Backward : MDIR_Forward);
    
    motor_pwm_r.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD * MOTOR_MAX_SPEED_LIMIT);
}

void motor_set_left_direction(MotorDir dir)
{
    motor_dir_l = static_cast<int>(dir);
}

void motor_set_right_direction(MotorDir dir)
{
    motor_dir_r = static_cast<int>(dir);
}


#ifdef __cplusplus
}
#endif