61 lines
1.5 KiB
C
Executable File
61 lines
1.5 KiB
C
Executable File
#include "motor.h"
|
|
#include "Clock.h"
|
|
#include <stdio.h>
|
|
|
|
|
|
|
|
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; // output
|
|
P3->OUT &= ~0xc0;// clear
|
|
|
|
P5->SEL0 &= ~0x30;
|
|
P5->SEL1 &= ~0x30;
|
|
P5->DIR |= 0x30;
|
|
P5->OUT &= ~0x30;
|
|
|
|
pwm_init_for_motors(MOTOR_PERIOD);
|
|
}
|
|
|
|
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;
|
|
} |