add initial labs

labs/lab_w4/main.c

@@ -0,0 +1,162 @@
+#include <stdio.h>
+#ifdef __MSP432P401R__
+#else
+#define __MSP432P401R__
+#endif
+
+//#define DEBUG_PRINT
+
+
+#include "Clock.h"
+#include "msp.h"
+
+#define LOOP_PERIOD_MS 20
+
+#define BASE_SPEED_PERCENT 30
+#define TURN_SPEED_PERCENT 25
+
+#define BASE_ON_TIME (LOOP_PERIOD_MS * BASE_SPEED_PERCENT / 100)
+#define BASE_OFF_TIME (LOOP_PERIOD_MS - BASE_ON_TIME)
+
+#define TURN_ON_TIME (LOOP_PERIOD_MS * TURN_SPEED_PERCENT / 100)
+#define TURN_OFF_TIME (LOOP_PERIOD_MS - TURN_ON_TIME)
+
+// IR
+void ir_init(void) {
+    P5->SEL0 &= ~0x08;
+    P5->SEL1 &= ~0x08;
+    P5->DIR |= 0x08;
+    P5->OUT &= ~0x08;
+    P9->SEL0 &= ~0x04;
+    P9->SEL1 &= ~0x04;
+    P9->DIR |= 0x04;
+    P9->OUT &= ~0x04;
+    P7->SEL0 &= ~0xff;
+    P7->SEL1 &= ~0xff;
+    P7->DIR &= ~0xff;
+}
+
+// Motor
+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 motor_move_forward() {
+    P5->OUT &= ~0b00110000;
+    P3->OUT |= 0b11000000;
+    P2->OUT |= 0b11000000;
+}
+
+void motor_move_biases_left() {
+    P5->OUT = (P5->OUT & ~0x30) | 0x10;
+    P3->OUT |= 0b11000000;
+    P2->OUT |= 0b11000000;
+}
+
+void motor_move_biases_right() {
+    P5->OUT = (P5->OUT & ~0x30) | 0x20;
+    P3->OUT |= 0b11000000;
+    P2->OUT |= 0b11000000;
+}
+
+void motor_move_stop() {
+    P2->OUT &= ~0xc0;
+}
+
+void ir_read_ready() {
+    P5->OUT |= 0x08;
+    P9->OUT |= 0x04;
+    P7->DIR = 0xff;
+    P7->OUT = 0xff;
+    Clock_Delay1us(10);
+    P7->DIR = 0x00;
+    Clock_Delay1us(1000);
+}
+
+void ir_read_value(char *value) {
+    int i;
+    uint8_t port_value = P7->IN;
+    for (i = 0; i < 8; i++) {
+        value[i] = !!(port_value & (0x01 << i));
+    }
+}
+
+void ir_read_off() {
+    P5->OUT &= ~0x08;
+    P9->OUT &= ~0x04;
+}
+
+void main(void) {
+    Clock_Init48MHz();
+    printf("start init\n");
+
+    //led_init();
+    ir_init();
+    motor_init();
+
+    printf("end init\n");
+
+    char ir_value[8];
+
+    uint32_t current_on_time = BASE_ON_TIME;
+    uint32_t current_off_time = BASE_OFF_TIME;
+
+    while (1) {
+        ir_read_ready();
+        ir_read_value(ir_value);
+        ir_read_off();
+        int i;
+#ifdef DEBUG_PRINT
+
+        for (i = 7; i >= 0; i--) { printf("%d", ir_value[i]); }
+        printf("\n");
+#endif
+        int cnt = 0;
+        for (i = 0; i < 8; i++) {
+            if (ir_value[i]) cnt++;
+        }
+
+        if (cnt >= 7) {
+            motor_move_stop();
+#ifdef DEBUG_PRINT
+            printf("T-detected.\n");
+#endif
+        } else if (ir_value[3] && ir_value[4]) {
+            motor_move_forward();
+            current_on_time = BASE_ON_TIME;
+            current_off_time = BASE_OFF_TIME;
+        } else if (ir_value[5] || ir_value[6] || ir_value[7]) {
+            motor_move_biases_left();
+            current_on_time = TURN_ON_TIME;
+            current_off_time = TURN_OFF_TIME;
+        } else if (ir_value[0] || ir_value[1] || ir_value[2]) {
+            motor_move_biases_right();
+            current_on_time = TURN_ON_TIME;
+            current_off_time = TURN_OFF_TIME;
+        } else {
+            motor_move_stop();
+            current_on_time = 0;
+            current_off_time = LOOP_PERIOD_MS;
+        }
+
+        if (current_on_time > 0) {
+            Clock_Delay1ms(current_on_time);
+            motor_move_stop();
+        }
+
+        Clock_Delay1ms(current_off_time);
+    }
+}