166 lines
5.2 KiB
C
166 lines
5.2 KiB
C
#include <stdbool.h>
|
|
#include "motor_simple.h"
|
|
|
|
#include "bump.h"
|
|
#include "systick.h"
|
|
|
|
#include "inc/msp432p401r.h"
|
|
|
|
#undef BIT0
|
|
#undef BIT1
|
|
#undef BIT2
|
|
#undef BIT3
|
|
#undef BIT4
|
|
#undef BIT5
|
|
#undef BIT6
|
|
#undef BIT7
|
|
|
|
#define BIT0 (uint8_t)(0x01)
|
|
#define BIT1 (uint8_t)(0x02)
|
|
#define BIT2 (uint8_t)(0x04)
|
|
#define BIT3 (uint8_t)(0x08)
|
|
#define BIT4 (uint8_t)(0x10)
|
|
#define BIT5 (uint8_t)(0x20)
|
|
#define BIT6 (uint8_t)(0x40)
|
|
#define BIT7 (uint8_t)(0x80)
|
|
|
|
// duty := H/(H+L) = H/10000
|
|
// H+L is always 10 000 microsecond
|
|
|
|
// Left motor (PH) direction connected to P5.4 (J3.29)
|
|
#define LEFT_DIR_PIN BIT4 // P5.4
|
|
// Right motor (PH) direction connected to P5.5 (J3.30)
|
|
#define RIGHT_DIR_PIN BIT5 // P5.5
|
|
|
|
// Left motor (EN) PWM connected to P2.7/TA0CCP4 (J4.40)
|
|
#define LEFT_PWM_PIN BIT7 // P2.7
|
|
// Right motor (EN) PWM connected to P2.6/TA0CCP3 (J4.39)
|
|
#define RIGHT_PWM_PIN BIT6 // P2.6
|
|
|
|
// Left motor (nSLEEP) enable connected to P3.7 (J4.31)
|
|
#define LEFT_SLEEP_PIN BIT7 // P3.7
|
|
// Right motor (nSLEEP) enable connected to P3.6 (J2.11)
|
|
#define RIGHT_SLEEP_PIN BIT6 // P3.6
|
|
|
|
|
|
// Initializes the 6 GPIO output lines and puts driver to sleep
|
|
void MotorInit(void) {
|
|
// 1. Configure all motor control pins as outputs
|
|
// P5.4 (Left DIR), P5.5 (Right DIR)
|
|
P5->SEL0 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN); // GPIO function
|
|
P5->SEL1 &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN);
|
|
P5->DIR |= (LEFT_DIR_PIN | RIGHT_DIR_PIN); // Output direction
|
|
|
|
// P3.6 (Right SLEEP), P3.7 (Left SLEEP)
|
|
P3->SEL0 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // GPIO function
|
|
P3->SEL1 &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN);
|
|
P3->DIR |= (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // Output direction
|
|
|
|
// P2.6 (Right EN), P2.7 (Left EN)
|
|
P2->SEL0 &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN); // GPIO function
|
|
P2->SEL1 &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN);
|
|
P2->DIR |= (LEFT_PWM_PIN | RIGHT_PWM_PIN); // Output direction
|
|
|
|
// 2. Initial state: disable motors
|
|
P3->OUT &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // SLEEP pins low - DRV8838 disabled
|
|
P2->OUT &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN); // EN pins low
|
|
|
|
SysTickWait1us(1000);
|
|
}
|
|
|
|
// Stops both motors, puts driver to sleep
|
|
void MotorStop(void) {
|
|
P3->OUT &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // SLEEP pins low
|
|
P2->OUT &= ~(LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // EN pins low
|
|
}
|
|
|
|
// Drives both motors forward at duty (100 to 9900)
|
|
// Runs for time duration, and then stops
|
|
// Stop the motors and return if any bumper switch is active
|
|
void MotorForward(uint16_t duty, uint32_t times_10ms) {
|
|
P5->OUT &= ~(LEFT_DIR_PIN | RIGHT_DIR_PIN); // set direction - PH low
|
|
P3->OUT |= (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // wake up motors
|
|
|
|
uint32_t i;
|
|
for (i = 0; i < times_10ms; ++i) {
|
|
P2->OUT |= (LEFT_PWM_PIN | RIGHT_PWM_PIN); // Enable both motors (EN high)
|
|
SysTickWait1us(duty);
|
|
|
|
P2->OUT &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN); // Disable motors
|
|
SysTickWait1us(10000 - duty);
|
|
|
|
uint8_t bump = BumpRead();
|
|
if ((~bump & BUMP_PINS)) break;
|
|
}
|
|
|
|
MotorStop();
|
|
}
|
|
|
|
// Drives both motors backward at duty (100 to 9900)
|
|
// Runs for time duration, and then stops
|
|
// Runs even if any bumper switch is active
|
|
void MotorBackward(uint16_t duty, uint32_t times_10ms) {
|
|
P5->OUT |= (LEFT_DIR_PIN | RIGHT_DIR_PIN); // set direction - PH high
|
|
P3->OUT |= (LEFT_SLEEP_PIN | RIGHT_SLEEP_PIN); // wake up motors
|
|
|
|
uint32_t i;
|
|
for (i = 0; i < times_10ms; ++i) {
|
|
P2->OUT |= (LEFT_PWM_PIN | RIGHT_PWM_PIN); // Enable both motors (EN high)
|
|
SysTickWait1us(duty);
|
|
|
|
P2->OUT &= ~(LEFT_PWM_PIN | RIGHT_PWM_PIN); // Disable motors
|
|
SysTickWait1us(10000 - duty);
|
|
|
|
uint8_t bump = BumpRead();
|
|
if ((~bump & BUMP_PINS)) break;
|
|
}
|
|
|
|
MotorStop();
|
|
}
|
|
|
|
// Drives just the left motor forward at duty (100 to 9900)
|
|
// Right motor is stopped (sleeping)
|
|
// Runs for time duration, and then stops
|
|
// Stop the motor and return if any bumper switch is active
|
|
void MotorLeft(uint16_t duty, uint32_t times_10ms) {
|
|
P5->OUT &= ~(LEFT_DIR_PIN); // set direction
|
|
P3->OUT |= (LEFT_SLEEP_PIN); // wake up motors
|
|
|
|
uint32_t i;
|
|
for (i = 0; i < times_10ms; ++i) {
|
|
P2->OUT |= (LEFT_PWM_PIN); // Enable both motors (EN high)
|
|
SysTickWait1us(duty);
|
|
|
|
P2->OUT &= ~(LEFT_PWM_PIN); // Disable motors
|
|
SysTickWait1us(10000 - duty);
|
|
|
|
uint8_t bump = BumpRead();
|
|
if ((~bump & BUMP_PINS)) break;
|
|
}
|
|
|
|
MotorStop();
|
|
}
|
|
|
|
// Drives just the right motor forward at duty (100 to 9900)
|
|
// Left motor is stopped (sleeping)
|
|
// Runs for time duration, and then stops
|
|
// Stop the motor and return if any bumper switch is active
|
|
void MotorRight(uint16_t duty, uint32_t times_10ms) {
|
|
P5->OUT &= ~(RIGHT_DIR_PIN); // set direction - PH high
|
|
P3->OUT |= (RIGHT_SLEEP_PIN); // wake up motors
|
|
|
|
uint32_t i;
|
|
for (i = 0; i < times_10ms; ++i) {
|
|
P2->OUT |= (RIGHT_PWM_PIN); // Enable both motors (EN high)
|
|
SysTickWait1us(duty);
|
|
|
|
P2->OUT &= ~(RIGHT_PWM_PIN); // Disable motors
|
|
SysTickWait1us(10000 - duty);
|
|
|
|
uint8_t bump = BumpRead();
|
|
if ((~bump & BUMP_PINS)) break;
|
|
}
|
|
|
|
MotorStop();
|
|
}
|