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@47:a682be9908b9, 2017-04-06 (annotated)

Committer:
Bobymicjohn
Date:
Thu Apr 06 17:55:40 2017 +0000
Revision:
47:a682be9908b9
Parent:
46:a5eb9bd3bb55
Child:
52:078b521c9edf
Added differential calculations in main.

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