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

assign VOLATILE V2

Browse Source
This commit is contained in:
yenru0
2025-12-10 02:26:47 +09:00
parent 2ca19bf287
commit 3d8b12c321
14 changed files with 348 additions and 888 deletions
Download Patch File Download Diff File Expand all files Collapse all files

226
ccs/motor.c
View File

@@ -2,198 +2,60 @@
#include "Clock.h"
#include <stdio.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) {
P2->SEL0 &= ~0xc0;
P2->SEL1 &= ~0xc0;
P2->DIR |= 0xc0; // output
P2->OUT &= ~0xc0;// clear
P3->SEL0 &= ~0xc0;
P3->SEL1 &= ~0xc0;
P3->DIR |= 0xc0;
P3->OUT &= ~0xc0;
P3->DIR |= 0xc0; // output
P3->OUT &= ~0xc0;// clear
P5->SEL0 &= ~0x30;
P5->SEL1 &= ~0x30;
P5->DIR |= 0x30;
P5->OUT &= ~0x30;
P5->DIR |= 0x30;
P5->OUT &= ~0x30;
P2->SEL0 &= ~0xc0;
P2->SEL1 &= ~0xc0;
P2->DIR |= 0xc0;
P2->OUT &= ~0xc0;
pwm_init_for_motors(MOTOR_PERIOD);
}
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() {
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);
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
}
void motor_move_backward(int speed) {
normalize_speed(&speed);
move_backward();
Clock_Delay1ms(speed);
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
}
void motor_turn_left(int speed) {
normalize_speed(&speed);
turn_left();
Clock_Delay1ms(speed);
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
}
void motor_turn_right(int speed) {
normalize_speed(&speed);
turn_right();
Clock_Delay1ms(speed);
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
}
void motor_rotate_clockwise(int speed) {
normalize_speed(&speed);
rotate_clockwise();
Clock_Delay1ms(speed);
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
set_dir_l('f');
set_dir_r('f');
// move_forward();
// motor_move_stop();
}
void motor_rotate_cclockwise(int speed) {
normalize_speed(&speed);
rotate_cclockwise();
Clock_Delay1ms(speed);
motor_move_stop();
Clock_Delay1ms(PERIOD_ms - speed);
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);
void motor_move(int32_t left, int32_t right) {
if (left == 0 && right == 0) {
pwm_set_duty_left(0);
pwm_set_duty_right(0);
} else if (left >= 0 && right >= 0) {
// fwd fwd
P3->OUT |= 0xc0;
P5->OUT &= ~0x30;
pwm_set_duty_left((uint16_t) left);
pwm_set_duty_right((uint16_t) right);
} else if (left < 0 && right < 0) {
// back back
P3->OUT |= 0xc0;
P5->OUT = 0x30;
pwm_set_duty_left((uint16_t) (-left));
pwm_set_duty_right((uint16_t) (-right));
} else if (left >= 0 && right < 0) {
// fwd back
P3->OUT |= 0xc0;
P5->OUT |= (P5->OUT & (~0x30)) | 0x10;
pwm_set_duty_left((uint16_t) left);
pwm_set_duty_right((uint16_t) (-right));
} else if (left < 0 && right >= 0) {
// back fwd
P3->OUT |= 0xc0;
P5->OUT |= (P5->OUT & (~0x30)) | 0x20;
pwm_set_duty_left((uint16_t) (-left));
pwm_set_duty_right((uint16_t) right);
}
}
void motor_stop(void) {
pwm_set_duty_left(0);
pwm_set_duty_right(0);
P3->OUT &= ~0xc0;
}