66 lines
1.0 KiB
C
66 lines
1.0 KiB
C
#include <stdio.h>
|
|
|
|
#include "Clock.h"
|
|
#include "msp.h"
|
|
|
|
#include "ir.h"
|
|
#include "motor.h"
|
|
#include "state.h"
|
|
#include "util.h"
|
|
|
|
#define LOOP_PERIOD_MS 20
|
|
|
|
const float Kp = 0.8f;
|
|
const float Kd = 4.5f;
|
|
|
|
void main(void) {
|
|
Clock_Init48MHz();
|
|
printf("Init Phase Begin\n");
|
|
|
|
ir_init();
|
|
motor_init();
|
|
|
|
SensorBuffer sb;
|
|
senbuf_init(&sb);
|
|
|
|
printf("Init Phase End\n");
|
|
|
|
uint8_t ir_value;
|
|
|
|
int error = 888481557;
|
|
int prev_error = 0;
|
|
|
|
int output_pid = 0;
|
|
|
|
int speed_left;
|
|
int speed_right;
|
|
|
|
while (1) {
|
|
ir_read_ready();
|
|
ir_read_value(ir_value);
|
|
ir_read_off();
|
|
int i, j;
|
|
|
|
senbuf_push(&sb, ir_value);
|
|
if(check_crossroad_robust(&sb)) {
|
|
// 교차로 처리 로직
|
|
move_stop();
|
|
while(1) {} // succ
|
|
}
|
|
|
|
|
|
|
|
error = get_error(ir_value);
|
|
output_pid = (int)(Kp * error + Kd * (error - prev_error));
|
|
|
|
|
|
// clamp;
|
|
|
|
// motor driverev_error = error;
|
|
|
|
Clock_Delay1ms(LOOP_PERIOD_MS);
|
|
|
|
|
|
}
|
|
}
|