initial commit

ccs/motor.c

@@ -0,0 +1,135 @@
+#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();
+}