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@100:ffbeefc9e218, 2017-04-20 (annotated)

Committer:
hazheng
Date:
Thu Apr 20 21:04:10 2017 +0000
Revision:
100:ffbeefc9e218
Parent:
87:15fcf7891bf9 
Better version of Intersection detection.

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