163 lines
3.4 KiB
C
163 lines
3.4 KiB
C
#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);
|
|
}
|
|
}
|