136 lines
2.4 KiB
C
136 lines
2.4 KiB
C
#include "motor.h"
|
|
#include "Clock.h"
|
|
|
|
// Period: 20ms
|
|
// MAX speed: 20
|
|
|
|
static void normalize_speed(int *speed) {
|
|
if (*speed <= 0) *speed = 0;
|
|
else if (*speed > MAX_SPEED) *speed = MAX_SPEED;
|
|
}
|
|
|
|
void motor_init(void) {
|
|
P3->SEL0 &= ~0xc0;
|
|
P3->SEL1 &= ~0xc0;
|
|
P3->DIR |= 0xc0;
|
|
P3->OUT &= ~0xc0;
|
|
|
|
P5->SEL0 &= ~0x30;
|
|
P5->SEL1 &= ~0x30;
|
|
P5->DIR |= 0x30;
|
|
P5->OUT &= ~0x30;
|
|
|
|
P2->SEL0 &= ~0xc0;
|
|
P2->SEL1 &= ~0xc0;
|
|
P2->DIR |= 0xc0;
|
|
P2->OUT &= ~0xc0;
|
|
}
|
|
|
|
void move_stop() {
|
|
P2->OUT &= ~0xc0;
|
|
}
|
|
|
|
static void move_forward() {
|
|
P5->OUT &= ~0b00110000;
|
|
P3->OUT |= 0b11000000;
|
|
P2->OUT |= 0b11000000;
|
|
}
|
|
|
|
static void move_backward() {
|
|
P5->OUT |= 0b00110000;
|
|
P3->OUT |= 0b11000000;
|
|
P2->OUT |= 0b11000000;
|
|
}
|
|
|
|
static void turn_left() {
|
|
P3->OUT |= 0b11000000;
|
|
P2->OUT &= ~0b10000000;
|
|
P2->OUT |= 0b01000000;
|
|
}
|
|
|
|
static void turn_right() {
|
|
P3->OUT |= 0b11000000;
|
|
P2->OUT &= ~0b01000000;
|
|
P2->OUT |= 0b10000000;
|
|
}
|
|
|
|
static void rotate_clockwise() {
|
|
P5->OUT &= ~0b00110000;
|
|
P5->OUT |= 0b00100000;
|
|
P3->OUT |= 0b11000000;
|
|
P2->OUT |= 0b11000000;
|
|
}
|
|
|
|
static void rotate_cclockwise() {
|
|
P5->OUT &= ~0b00110000;
|
|
P5->OUT |= 0b00010000;
|
|
P3->OUT |= 0b11000000;
|
|
P2->OUT |= 0b11000000;
|
|
}
|
|
|
|
void motor_move_forward(int speed) {
|
|
normalize_speed(&speed);
|
|
|
|
move_forward();
|
|
Clock_Delay1ms(speed);
|
|
|
|
move_stop();
|
|
Clock_Delay1ms(PERIOD_ms - speed);
|
|
}
|
|
|
|
void motor_move_backward(int speed) {
|
|
normalize_speed(&speed);
|
|
|
|
move_backward();
|
|
Clock_Delay1ms(speed);
|
|
|
|
move_stop();
|
|
Clock_Delay1ms(PERIOD_ms - speed);
|
|
}
|
|
|
|
void motor_turn_left(int speed) {
|
|
normalize_speed(&speed);
|
|
|
|
turn_left();
|
|
Clock_Delay1ms(speed);
|
|
|
|
move_stop();
|
|
Clock_Delay1ms(PERIOD_ms - speed);
|
|
}
|
|
|
|
void motor_turn_right(int speed) {
|
|
normalize_speed(&speed);
|
|
|
|
turn_right();
|
|
Clock_Delay1ms(speed);
|
|
|
|
move_stop();
|
|
Clock_Delay1ms(PERIOD_ms - speed);
|
|
}
|
|
|
|
void motor_rotate_clockwise(int speed) {
|
|
normalize_speed(&speed);
|
|
|
|
rotate_clockwise();
|
|
Clock_Delay1ms(speed);
|
|
|
|
move_stop();
|
|
Clock_Delay1ms(PERIOD_ms - speed);
|
|
|
|
move_forward();
|
|
move_stop();
|
|
}
|
|
|
|
void motor_rotate_cclockwise(int speed) {
|
|
normalize_speed(&speed);
|
|
|
|
rotate_cclockwise();
|
|
Clock_Delay1ms(speed);
|
|
|
|
move_stop();
|
|
Clock_Delay1ms(PERIOD_ms - speed);
|
|
|
|
move_forward();
|
|
move_stop();
|
|
}
|