/** Main loop. */
static void
main_loop (void)
{
main_timer[3] = timer_read ();
/* Compute absolute position. */
aux_pos_update ();
main_timer[4] = timer_read ();
/* Compute trajectory. */
aux_traj_update ();
/* Prepare control system. */
cs_update_prepare ();
main_timer[5] = timer_read ();
/* Wait for next cycle. */
timer_wait ();
/* Encoder update. */
encoder_update ();
main_timer[0] = timer_read ();
/* Control system update. */
cs_update ();
main_timer[1] = timer_read ();
/* Pwm setup. */
output_update ();
main_timer[2] = timer_read ();
/* Sequences. */
seq_update (&seq_aux[0], &cs_aux[0].state);
seq_update (&seq_aux[1], &cs_aux[1].state);
/* Stats. */
if (main_sequence_ack
&& (seq_aux[0].ack != seq_aux[0].finish
|| seq_aux[1].ack != seq_aux[1].finish)
&& !--main_sequence_ack_cpt)
{
//XXX here
proto_send2b ('A', seq_aux[0].finish, seq_aux[1].finish);
main_sequence_ack_cpt = main_sequence_ack;
}
if (main_stat_counter && !--main_stat_counter_cpt)
{
proto_send2w ('C', encoder_aux[0].cur, encoder_aux[1].cur);
main_stat_counter_cpt = main_stat_counter;
}
if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
{
proto_send2w ('Y', aux[0].pos, aux[1].pos);
main_stat_aux_pos_cpt = main_stat_aux_pos;
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
proto_send2w ('S', cs_aux[0].speed.cur_f >> 8,
cs_aux[1].speed.cur_f >> 8);
main_stat_speed_cpt = main_stat_speed;
}
static void control_task(void)
{
static int msec = 0;
int left_encoder_count;
int right_encoder_count;
int i;
long translational_velocity;
long rotational_velocity;
unsigned char current_interrupt_priority = get_imask_exr();
// タイムスタンプの更新
run_system_increment_timestamp(&run_->run_system);
// CONTROL_CYCLE_MSEC 毎に処理を行う
if (++msec >= CONTROL_CYCLE_MSEC) {
msec = 0;
} else {
return;
}
// エンコーダ値の更新
set_imask_exr(INTERRUPT_PRIORITY_ALL_MASK);
for (i = 0; i < NUMBER_OF_WHEELS; ++i) {
encoder_update(&run_->wheel[i].encoder);
}
set_imask_exr(current_interrupt_priority);
// 推定自己位置の更新
left_encoder_count = encoder_difference(&run_->wheel[LEFT_WHEEL].encoder);
right_encoder_count = encoder_difference(&run_->wheel[RIGHT_WHEEL].encoder);
odometry_update(&run_->odometry, +right_encoder_count, -left_encoder_count);
// 走行経路の指示
switch (run_->run_system.mode) {
case NORMAL_CONTROL:
path_follow_update(&translational_velocity,
&rotational_velocity, &run_->path, &run_->odometry);
body_set_velocity(run_->wheel,
translational_velocity, rotational_velocity);
break;
case DIRECT_WHEEL_CONTROL:
// 何も変更しない
break;
}
// 設定された車輪速になるように制御する
for (i = 0; i < NUMBER_OF_WHEELS; ++i) {
wheel_velocity_control(&run_->wheel[i]);
}
}
void update_pid_outout(void)
{
if(speed_active)
speed_active--;
else
{
leftPID.AimEncoder = 0;
rightPID.AimEncoder = 0;
}
encoder_update();
doPID(&rightPID);
doPID(&leftPID);
set_motor_speeds(leftPID.output, rightPID.output);
}
uint8_t matrix_scan(void) {
// dip switch
dip_switch[0] = !palReadPad(GPIOB, 14);
dip_switch[1] = !palReadPad(GPIOA, 15);
dip_switch[2] = !palReadPad(GPIOA, 10);
dip_switch[3] = !palReadPad(GPIOB, 9);
for (uint8_t i = 0; i < 4; i++) {
if (last_dip_switch[i] ^ dip_switch[i])
dip_update(i, dip_switch[i]);
}
memcpy(last_dip_switch, dip_switch, sizeof(&dip_switch));
// encoder on B12 and B13
encoder_state <<= 2;
encoder_state |= (palReadPad(GPIOB, 12) << 0) | (palReadPad(GPIOB, 13) << 1);
encoder_value += encoder_LUT[encoder_state & 0xF];
if (encoder_value >= ENCODER_RESOLUTION) {
encoder_update(0);
}
if (encoder_value <= -ENCODER_RESOLUTION) { // direction is arbitrary here, but this clockwise
encoder_update(1);
}
encoder_value %= ENCODER_RESOLUTION;
// actual matrix
for (int col = 0; col < MATRIX_COLS; col++) {
matrix_col_t data = 0;
// strobe col { B11, B10, B2, B1, A7, B0 }
switch (col) {
case 0: palSetPad(GPIOB, 11); break;
case 1: palSetPad(GPIOB, 10); break;
case 2: palSetPad(GPIOB, 2); break;
case 3: palSetPad(GPIOB, 1); break;
case 4: palSetPad(GPIOA, 7); break;
case 5: palSetPad(GPIOB, 0); break;
}
// need wait to settle pin state
wait_us(20);
// read row data { A10, A9, A8, B15, C13, C14, C15, A2 }
data = (
(palReadPad(GPIOA, 10) << 0 ) |
(palReadPad(GPIOA, 9) << 1 ) |
(palReadPad(GPIOA, 8) << 2 ) |
(palReadPad(GPIOB, 15) << 3 ) |
(palReadPad(GPIOC, 13) << 4 ) |
(palReadPad(GPIOC, 14) << 5 ) |
(palReadPad(GPIOC, 15) << 6 ) |
(palReadPad(GPIOA, 2) << 7 ) |
(palReadPad(GPIOA, 3) << 8 ) |
(palReadPad(GPIOA, 6) << 9 )
);
// unstrobe col { B11, B10, B2, B1, A7, B0 }
switch (col) {
case 0: palClearPad(GPIOB, 11); break;
case 1: palClearPad(GPIOB, 10); break;
case 2: palClearPad(GPIOB, 2); break;
case 3: palClearPad(GPIOB, 1); break;
case 4: palClearPad(GPIOA, 7); break;
case 5: palClearPad(GPIOB, 0); break;
}
if (matrix_debouncing[col] != data) {
matrix_debouncing[col] = data;
debouncing = true;
debouncing_time = timer_read();
}
}
if (debouncing && timer_elapsed(debouncing_time) > DEBOUNCE) {
for (int row = 0; row < MATRIX_ROWS; row++) {
matrix[row] = 0;
for (int col = 0; col < MATRIX_COLS; col++) {
matrix[row] |= ((matrix_debouncing[col] & (1 << row) ? 1 : 0) << col);
}
}
debouncing = false;
}
matrix_scan_quantum();
return 1;
}
int
main (void)
{
uint8_t i;
#ifndef HOST
uint8_t read_old = 0;
uint8_t old_ind = 0;
const int total = 5000;
#endif
timer_init ();
for (i = 0; i < AC_ENCODER_EXT_NB; i++)
encoder_init (i, &encoder[i]);
encoder_corrector_init (&encoder_corrector_right);
uart0_init ();
proto_send0 ('z');
sei ();
while (1)
{
timer_wait ();
if (count)
{
encoder_update ();
encoder_corrector_update (&encoder_corrector_right, &encoder[1]);
}
#ifndef HOST
if (read && !--read_cpt)
{
uint8_t r0, r1, r2, r3;
r0 = encoder_ext_read (0);
r1 = encoder_ext_read (1);
r2 = encoder_ext_read (2);
r3 = encoder_ext_read (3);
if (read_mode == 0 || (read_mode == 1 && r3 != read_old)
|| (read_mode == 2
&& (r0 == 0 || r1 == 0 || r2 == 0 || r3 == 0)))
{
proto_send4b ('r', r0, r1, r2, r3);
read_old = r3;
}
read_cpt = read;
}
if (ind && !--ind_cpt)
{
i = encoder_ext_read (3);
if (!ind_init && i != old_ind)
{
uint8_t eip = old_ind + total;
uint8_t eim = old_ind - total;
proto_send7b ('i', old_ind, i, eip, eim, i - eip, i - eim,
i == eip || i == eim);
}
old_ind = i;
ind_init = 0;
ind_cpt = ind;
}
#endif
if (count && !--count_cpt)
{
proto_send4w ('C', encoder[0].cur, encoder[1].cur,
encoder[2].cur, encoder[3].cur);
count_cpt = count;
}
while (uart0_poll ())
proto_accept (uart0_getc ());
}
}
uint8_t matrix_scan(void) {
// encoder on B13 and B14
encoder_state <<= 2;
encoder_state |= (palReadPad(GPIOB, 13) << 0) | (palReadPad(GPIOB, 14) << 1);
encoder_value += encoder_LUT[encoder_state & 0xF];
if (encoder_value >= ENCODER_RESOLUTION) {
encoder_update(0);
}
if (encoder_value <= -ENCODER_RESOLUTION) { // direction is arbitrary here, but this clockwise
encoder_update(1);
}
encoder_value %= ENCODER_RESOLUTION;
// actual matrix
for (int col = 0; col < MATRIX_COLS; col++) {
matrix_row_t data = 0;
// strobe col { PB8, PB2, PB10, PA0, PA1, PA2, PB0, PA3, PB1, PA6, PA7, PB1, PA6, PA7, PB12, PC3, PB11, }
switch (col) {
case 0: palSetPad(GPIOB, 8); break;
case 1: palSetPad(GPIOB, 2); break;
case 2: palSetPad(GPIOB, 10); break;
case 3: palSetPad(GPIOA, 0); break;
case 4: palSetPad(GPIOA, 1); break;
case 5: palSetPad(GPIOA, 2); break;
case 6: palSetPad(GPIOB, 0); break;
case 7: palSetPad(GPIOA, 3); break;
case 8: palSetPad(GPIOB, 1); break;
case 9: palSetPad(GPIOA, 6); break;
case 10: palSetPad(GPIOA, 7); break;
case 11: palSetPad(GPIOB, 12); break;
case 12: palSetPad(GPIOC, 13); break;
case 13: palSetPad(GPIOB, 11); break;
case 14: palSetPad(GPIOB, 9); break;
}
// need wait to settle pin state
wait_us(20);
// read row data { PC15, PC14, PA10, PA9, PA8 }
data = (
(palReadPad(GPIOC, 15) << 0 ) |
(palReadPad(GPIOC, 14) << 1 ) |
(palReadPad(GPIOA, 10) << 2 ) |
(palReadPad(GPIOA, 9) << 3 ) |
(palReadPad(GPIOA, 8) << 4 )
);
// unstrobe col { PB8, PB2, PB10, PA0, PA1, PA2, PB0, PA3, PB1, PA6, PA7, PB1, PA6, PA7, PB12, PC3, PB11, }
switch (col) {
case 0: palClearPad(GPIOB, 8); break;
case 1: palClearPad(GPIOB, 2); break;
case 2: palClearPad(GPIOB, 10); break;
case 3: palClearPad(GPIOA, 0); break;
case 4: palClearPad(GPIOA, 1); break;
case 5: palClearPad(GPIOA, 2); break;
case 6: palClearPad(GPIOB, 0); break;
case 7: palClearPad(GPIOA, 3); break;
case 8: palClearPad(GPIOB, 1); break;
case 9: palClearPad(GPIOA, 6); break;
case 10: palClearPad(GPIOA, 7); break;
case 11: palClearPad(GPIOB, 12); break;
case 12: palClearPad(GPIOC, 13); break;
case 13: palClearPad(GPIOB, 11); break;
case 14: palClearPad(GPIOB, 9); break;
}
if (matrix_debouncing[col] != data) {
matrix_debouncing[col] = data;
debouncing = true;
debouncing_time = timer_read();
}
}
if (debouncing && timer_elapsed(debouncing_time) > DEBOUNCE) {
for (int row = 0; row < MATRIX_ROWS; row++) {
matrix[row] = 0;
for (int col = 0; col < MATRIX_COLS; col++) {
matrix[row] |= ((matrix_debouncing[col] & (1 << row) ? 1 : 0) << col);
}
}
debouncing = false;
}
matrix_scan_quantum();
return 1;
}
void in_encoder_read_and_update()
{
int1 channelA = input_state(PI_ENCODER_A_PIN);
int1 channelB = input_state(PI_ENCODER_B_PIN);
encoder_update( channelA, channelB );
}
