yenru0/2025-02-MPX
SHA256
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update motor module

Browse Source
This commit is contained in:
yenru0
2025-12-03 05:01:31 +09:00
parent 9c2fdbf8ed
commit 2c503a6830
2 changed files with 78 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
ccs/motor.c
View File

@@ -26,8 +26,30 @@ void motor_init(void) {
P2->OUT &= ~0xc0;
}
void move_stop() {
P2->OUT &= ~0xc0;
static void set_dir_l(char dir) { // f: forward, b: backward
if (dir == 'f') P5->OUT &= ~0b00010000;
else if (dir == 'b') P5->OUT |= 0b00010000;
}
static void set_dir_r(char dir) { // f: forward, b: backward
if (dir == 'f') P5->OUT &= ~0b00100000;
else if (dir == 'b') P5->OUT |= 0b00100000;
}
static void set_en_l(int enable) { // 1: on, 0: off
if (enable == 1) P2->OUT |= 0b10000000;
else if (enable == 0) P2->OUT &= ~0b10000000;
}
static void set_en_r(int enable) { // 1: on, 0: off
if (enable == 1) P2->OUT |= 0b01000000;
else if (enable == 0) P2->OUT &= ~0b01000000;
}
void motor_move_stop() {
// P2->OUT &= ~0xc0;
set_en_l(0);
set_en_r(0);
}
static void move_forward() {
@@ -74,7 +96,7 @@ void motor_move_forward(int speed) {
move_forward();
Clock_Delay1ms(speed);
move_stop();
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
}
@@ -84,7 +106,7 @@ void motor_move_backward(int speed) {
move_backward();
Clock_Delay1ms(speed);
move_stop();
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
}
@@ -94,7 +116,7 @@ void motor_turn_left(int speed) {
turn_left();
Clock_Delay1ms(speed);
move_stop();
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
}
@@ -104,7 +126,7 @@ void motor_turn_right(int speed) {
turn_right();
Clock_Delay1ms(speed);
move_stop();
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
}
@@ -114,11 +136,13 @@ void motor_rotate_clockwise(int speed) {
rotate_clockwise();
Clock_Delay1ms(speed);
move_stop();
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
move_forward();
move_stop();
set_dir_l('f');
set_dir_r('f');
// move_forward();
// motor_move_stop();
}
void motor_rotate_cclockwise(int speed) {
@@ -127,9 +151,49 @@ void motor_rotate_cclockwise(int speed) {
rotate_cclockwise();
Clock_Delay1ms(speed);
move_stop();
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
move_forward();
move_stop();
set_dir_l('f');
set_dir_r('f');
// move_forward();
// motor_move_stop();
}
void motor_move_custom(int left_speed, int right_speed) { // positive will be forward, negative will be backward
int l_speed = left_speed > 0 ? left_speed : -left_speed;
int r_speed = right_speed > 0 ? right_speed : -right_speed;
normalize_speed(&l_speed);
normalize_speed(&r_speed);
if (left_speed >= 0) set_dir_l('f');
else if (left_speed < 0) set_dir_l('b');
if (right_speed >= 0) set_dir_r('f');
else if (right_speed < 0) set_dir_r('b');
if (l_speed >= r_speed) {
set_en_l(1);
set_en_r(1);
Clock_Delay1ms(r_speed);
set_en_r(0);
Clock_Delay1ms(l_speed - r_speed);
set_en_l(0);
Clock_Delay1ms(PERIOD_ms - l_speed);
} else if (l_speed < r_speed) {
set_en_l(1);
set_en_r(1);
Clock_Delay1ms(l_speed);
set_en_l(0);
Clock_Delay1ms(r_speed - l_speed);
set_en_r(0);
Clock_Delay1ms(PERIOD_ms - r_speed);
}
}