static void motor_info_get(struct hj_pktc_motor_info *m, uint16_t current,
uint8_t i)
{
m->current = htons(current);
m->pwr = htons(motor_pwr[i]);
enc_get(&m->e, i);
}
int strategy_step() {
int next = 1;
struct wpt *goal;
while (next) {
printf("STRATEGY: %i %i %i\n",cycle_position, cycle[cycle_position], strategy_position);
next = 0;
goal = strat[cycle[cycle_position]] + strategy_position;
printf("use wpt %d,%d,%d,%d,%d\n", goal->x,goal->y,goal->rev,goal->unload,goal->sleep);
if (goal->unload) {
enc_type pos;
enc_get(&pos);
if (!((pos.pos_x > goal->x) || (pos.pos_y > goal->y))) {
do_servo(goal);
}
else {
u16 r,g,b,w;
char col='N';
i2c_taos_get_color();
usleep(1000*130);
if (i2c_taos_fetch_color(&r, &g, &b, &w) == ourcolor)
do_servo(goal);
}
next = 1;
}
else if ((goal->rev&3) == 0) {
i2c_taos_sort_enable();
motor_command(goal->x, goal->y, 0, 0, pos_error_allow);
}
else if ((goal->rev&3) == 1) {
i2c_taos_sort_enable();
motor_command(goal->x, goal->y, 1, 0, pos_error_allow);
}
else if ((goal->rev&3) == 2) {
i2c_taos_sort_disable();
motor_command(goal->x, goal->y, 0, 0, pos_error_allow);
}
else if ((goal->rev&3) == 3) {
i2c_taos_sort_disable();
motor_command(goal->x, goal->y, 1, 0, pos_error_allow);
}
strategy_position++;
if (strategy_position > strat[cycle[cycle_position]][0].x) {
strategy_position = 1;
cycle_position++;
if (cycle_position >= cycle_size)
cycle_position = 0;
}
}
printf("STRATEGY: done\n");
return (goal->rev >> 2)&1;
}
void gcb_set_enc(GtkWidget *w, struct encdata *enc) {
/* fill muse-core */
enc_get(enc);
/* debug */
func("gcb_set_enc: outchan->quality(%.1f)", enc->outchan->_quality);
func("gcb_set_enc: outchan->bps(%d)", enc->outchan->bps());
func("gcb_set_enc: outchan->channels(%i)", enc->outchan->channels());
func("gcb_set_enc: outchan->freq(%d)", enc->outchan->freq());
func("gcb_set_enc: outchan->highpass(%d)", enc->outchan->highpass());
func("gcb_set_enc: outchan->lowpass(%d)", enc->outchan->lowpass());
mixer->apply_enc( enc->outchan->id );
}
void controlpanel_motor() {
bool motenables[4] = {0, 0, 0, 0};
int16_t PWM = 0;
while (true) {
char ch = controlpanel_promptChar("Motor");
if (ch >= '0' && ch <= '3') {
int num = ch-'0';
bool &enable = motenables[num];
enable = !enable;
printf_P(PSTR("Motor %d %s\n"), num, enable ? "enabled" : "disabled");
} else if (ch == 'e') {
printf_P(PSTR("L %i R %i\n"), enc_get(MOTOR_LEFT), enc_get(MOTOR_RIGHT));
} else if (ch == 'E') {
printf_P(PSTR("Encoders reset\n"));
enc_reset(MOTOR_LEFT);
enc_reset(MOTOR_RIGHT);
} else {
switch (ch) {
case 'x':
PWM += 50;
break;
case 'z':
PWM -= 50;
break;
case 'X':
PWM += 200;
break;
case 'Z':
PWM -= 200;
break;
case 'a':
PWM = -motor_maxPWM;
break;
case 's':
PWM = motor_maxPWM;
break;
case 'd':
PWM = -PWM;
break;
case ' ':
PWM = 0;
break;
case 'q':
motor_allOff();
return;
default:
puts_P(unknown_str);
break;
case '?':
static const char msg[] PROGMEM =
"Motor commands:\n"
" 0123 - Toggle motors\n"
" zxZX - Adjust PWM\n"
" d - Flip PWM direction\n"
" space - Zero PWM\n"
" as - Max PWM\n"
" e - Display encoders\n"
" q - Back";
puts_P(msg);
break;
}
if (PWM > motor_maxPWM)
PWM = motor_maxPWM;
else if (PWM < -motor_maxPWM)
PWM = -motor_maxPWM;
printf_P(PSTR("PWM %d\n"), PWM);
}
for (uint8_t i=0; i<4; i++)
motor_setPWM(i, motenables[i] ? PWM : 0);
}
}
