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@44:15de535c4005, 2017-03-29 (annotated)

Committer:
hazheng
Date:
Wed Mar 29 21:19:27 2017 +0000
Revision:
44:15de535c4005
Parent:
13:7dcb1642ef99
Child:
46:a5eb9bd3bb55
Made the car move!...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bobymicjohn 4:25e028102625 1 #include "Motor.h"
Bobymicjohn 4:25e028102625 2 #include "mbed.h"
Bobymicjohn 8:92f6baeea027 3
Bobymicjohn 11:676ea42afd56 4 #include "Core.h"
hazheng 44:15de535c4005 5 #include "SWUSBServer.h"
Bobymicjohn 11:676ea42afd56 6
Bobymicjohn 8:92f6baeea027 7 #include "PinAssignment.h"
Bobymicjohn 4:25e028102625 8
hazheng 44:15de535c4005 9 #define MOTOR_PERIOD 0.002
hazheng 44:15de535c4005 10
Bobymicjohn 11:676ea42afd56 11 Motor::Motor(SW::Core& core) :
Bobymicjohn 11:676ea42afd56 12 m_core(core),
hazheng 44:15de535c4005 13 m_dirL(DigitalOut(PIN_MC_DIR_L, MDIR_Forward)),
hazheng 44:15de535c4005 14 m_dirR(DigitalOut(PIN_MC_DIR_R, MDIR_Forward)),
hazheng 13:7dcb1642ef99 15 m_pwmL(PwmOut(PIN_MC_SPEED_L)),
hazheng 13:7dcb1642ef99 16 m_pwmR(PwmOut(PIN_MC_SPEED_R))
Bobymicjohn 8:92f6baeea027 17
Bobymicjohn 8:92f6baeea027 18 {
hazheng 44:15de535c4005 19 m_pwmL.period(MOTOR_PERIOD);
hazheng 44:15de535c4005 20 m_pwmR.period(MOTOR_PERIOD);
hazheng 44:15de535c4005 21 m_pwmL = 0.0f;
hazheng 44:15de535c4005 22 m_pwmR = 0.0f;
Bobymicjohn 8:92f6baeea027 23 }
Bobymicjohn 4:25e028102625 24
Bobymicjohn 8:92f6baeea027 25
Bobymicjohn 8:92f6baeea027 26 void Motor::Update(float deltaTime)
Bobymicjohn 4:25e028102625 27 {
Bobymicjohn 4:25e028102625 28
Bobymicjohn 4:25e028102625 29 }
Bobymicjohn 8:92f6baeea027 30
hazheng 44:15de535c4005 31 void Motor::setLeftSpeed(const float speed)
Bobymicjohn 8:92f6baeea027 32 {
Bobymicjohn 8:92f6baeea027 33 if(speed < -1.0f || speed > 1.0f)
Bobymicjohn 8:92f6baeea027 34 return;
Bobymicjohn 4:25e028102625 35
hazheng 44:15de535c4005 36 //m_pwmL.period_ms(MOTOR_PERIOD);
Bobymicjohn 8:92f6baeea027 37
Bobymicjohn 8:92f6baeea027 38 setLeftDirection(speed < 0 ? MDIR_Backward : MDIR_Forward);
hazheng 44:15de535c4005 39 m_pwmL.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
hazheng 44:15de535c4005 40 //m_pwmL = (speed < 0 ? -speed : speed);
Bobymicjohn 8:92f6baeea027 41
hazheng 44:15de535c4005 42 char buf[20];
hazheng 44:15de535c4005 43 sprintf(buf, "L %f-%f", speed, (float)m_pwmL);
hazheng 44:15de535c4005 44 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 44:15de535c4005 45 //m_pwmL = speed;
Bobymicjohn 4:25e028102625 46 }
Bobymicjohn 4:25e028102625 47
hazheng 44:15de535c4005 48 void Motor::setRightSpeed(const float speed)
Bobymicjohn 4:25e028102625 49 {
Bobymicjohn 8:92f6baeea027 50 if(speed < -1.0f || speed > 1.0f)
Bobymicjohn 8:92f6baeea027 51 return;
Bobymicjohn 4:25e028102625 52
hazheng 44:15de535c4005 53 //m_pwmR.period_ms(MOTOR_PERIOD);
Bobymicjohn 8:92f6baeea027 54
Bobymicjohn 8:92f6baeea027 55 setRightDirection(speed < 0 ? MDIR_Backward : MDIR_Forward);
hazheng 44:15de535c4005 56 m_pwmR.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
hazheng 44:15de535c4005 57 //m_pwmR = (speed < 0 ? -speed : speed);
hazheng 44:15de535c4005 58
hazheng 44:15de535c4005 59 char buf[20];
hazheng 44:15de535c4005 60 sprintf(buf, "R %f-%f", speed, (float)m_pwmR);
hazheng 44:15de535c4005 61 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
Bobymicjohn 4:25e028102625 62
hazheng 44:15de535c4005 63 //m_pwmR = speed;
Bobymicjohn 4:25e028102625 64 }
Bobymicjohn 4:25e028102625 65
hazheng 44:15de535c4005 66 void Motor::setSpeeds(const float leftSpeed, const float rightSpeed)
Bobymicjohn 4:25e028102625 67 {
Bobymicjohn 4:25e028102625 68 setLeftSpeed(leftSpeed);
Bobymicjohn 4:25e028102625 69 setRightSpeed(rightSpeed);
Bobymicjohn 4:25e028102625 70 }
Bobymicjohn 8:92f6baeea027 71
Bobymicjohn 8:92f6baeea027 72
Bobymicjohn 8:92f6baeea027 73 void Motor::setLeftDirection(MotorDir dir)
Bobymicjohn 8:92f6baeea027 74 {
Bobymicjohn 8:92f6baeea027 75 m_dirL = static_cast<int>(dir);
Bobymicjohn 8:92f6baeea027 76 }
Bobymicjohn 8:92f6baeea027 77
Bobymicjohn 8:92f6baeea027 78 void Motor::setRightDirection(MotorDir dir)
Bobymicjohn 8:92f6baeea027 79 {
Bobymicjohn 8:92f6baeea027 80 m_dirR = static_cast<int>(dir);
Bobymicjohn 8:92f6baeea027 81 }
Bobymicjohn 8:92f6baeea027 82
Bobymicjohn 8:92f6baeea027 83 void Motor::setDirections(MotorDir dirL, MotorDir dirR)
Bobymicjohn 8:92f6baeea027 84 {
Bobymicjohn 8:92f6baeea027 85 setLeftDirection(dirL);
Bobymicjohn 8:92f6baeea027 86 setRightDirection(dirR);
Bobymicjohn 8:92f6baeea027 87 }