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@52:078b521c9edf, 2017-04-06 (annotated)

Committer:
hazheng
Date:
Thu Apr 06 18:57:54 2017 +0000
Revision:
52:078b521c9edf
Parent:
47:a682be9908b9
Child:
87:15fcf7891bf9
Clean the rear differential code.

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"
hazheng 46:a5eb9bd3bb55 8 #include "GlobalVariable.h"
Bobymicjohn 4:25e028102625 9
hazheng 52:078b521c9edf 10 #define MOTOR_PERIOD 0.020f
hazheng 46:a5eb9bd3bb55 11
Bobymicjohn 47:a682be9908b9 12
hazheng 46:a5eb9bd3bb55 13
hazheng 46:a5eb9bd3bb55 14 #ifdef __cplusplus
hazheng 46:a5eb9bd3bb55 15 extern "C" {
hazheng 46:a5eb9bd3bb55 16 #endif
hazheng 44:15de535c4005 17
hazheng 46:a5eb9bd3bb55 18 static DigitalOut motor_dir_l(PIN_MC_DIR_L, MDIR_Forward);
hazheng 46:a5eb9bd3bb55 19 static DigitalOut motor_dir_r(PIN_MC_DIR_R, MDIR_Forward);
hazheng 46:a5eb9bd3bb55 20
hazheng 46:a5eb9bd3bb55 21 static PwmOut motor_pwm_l(PIN_MC_SPEED_L);
hazheng 46:a5eb9bd3bb55 22 static PwmOut motor_pwm_r(PIN_MC_SPEED_R);
Bobymicjohn 8:92f6baeea027 23
hazheng 46:a5eb9bd3bb55 24 void motor_init()
Bobymicjohn 8:92f6baeea027 25 {
hazheng 46:a5eb9bd3bb55 26 motor_pwm_l.period(MOTOR_PERIOD);
hazheng 46:a5eb9bd3bb55 27 motor_pwm_r.period(MOTOR_PERIOD);
hazheng 46:a5eb9bd3bb55 28 motor_dir_l = 0.0f;
hazheng 46:a5eb9bd3bb55 29 motor_dir_r = 0.0f;
Bobymicjohn 8:92f6baeea027 30 }
Bobymicjohn 4:25e028102625 31
hazheng 46:a5eb9bd3bb55 32 void motor_set_left_speed(const float speed)
Bobymicjohn 8:92f6baeea027 33 {
Bobymicjohn 8:92f6baeea027 34 if(speed < -1.0f || speed > 1.0f)
Bobymicjohn 8:92f6baeea027 35 return;
Bobymicjohn 4:25e028102625 36
hazheng 46:a5eb9bd3bb55 37 motor_set_left_direction(speed < 0 ? MDIR_Backward : MDIR_Forward);
hazheng 46:a5eb9bd3bb55 38
hazheng 52:078b521c9edf 39 motor_pwm_l.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD * MOTOR_MAX_SPEED_LIMIT);
Bobymicjohn 8:92f6baeea027 40
hazheng 46:a5eb9bd3bb55 41 //char buf[20];
hazheng 46:a5eb9bd3bb55 42 //sprintf(buf, "Motor %f", (float)motor_pwm_l);
hazheng 46:a5eb9bd3bb55 43 //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
Bobymicjohn 4:25e028102625 44 }
Bobymicjohn 4:25e028102625 45
hazheng 46:a5eb9bd3bb55 46 void motor_set_right_speed(const float speed)
Bobymicjohn 4:25e028102625 47 {
Bobymicjohn 8:92f6baeea027 48 if(speed < -1.0f || speed > 1.0f)
Bobymicjohn 8:92f6baeea027 49 return;
Bobymicjohn 4:25e028102625 50
hazheng 46:a5eb9bd3bb55 51 motor_set_right_direction(speed < 0 ? MDIR_Backward : MDIR_Forward);
hazheng 44:15de535c4005 52
hazheng 52:078b521c9edf 53 motor_pwm_r.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD * MOTOR_MAX_SPEED_LIMIT);
hazheng 46:a5eb9bd3bb55 54 }
Bobymicjohn 4:25e028102625 55
hazheng 46:a5eb9bd3bb55 56 void motor_set_left_direction(MotorDir dir)
Bobymicjohn 4:25e028102625 57 {
hazheng 46:a5eb9bd3bb55 58 motor_dir_l = static_cast<int>(dir);
Bobymicjohn 4:25e028102625 59 }
Bobymicjohn 8:92f6baeea027 60
hazheng 46:a5eb9bd3bb55 61 void motor_set_right_direction(MotorDir dir)
Bobymicjohn 8:92f6baeea027 62 {
hazheng 46:a5eb9bd3bb55 63 motor_dir_r = static_cast<int>(dir);
Bobymicjohn 8:92f6baeea027 64 }
hazheng 46:a5eb9bd3bb55 65
hazheng 46:a5eb9bd3bb55 66
hazheng 46:a5eb9bd3bb55 67 #ifdef __cplusplus
Bobymicjohn 8:92f6baeea027 68 }
hazheng 46:a5eb9bd3bb55 69 #endif