int umain (void) {
encoder_reset(RIGHT_ENCODER);
encoder_reset(LEFT_ENCODER);
while(1)
{
motor_set_vel(LEFT_MOTOR, TEST_SPEED);
motor_set_vel(RIGHT_MOTOR, 0);
printf("\nRight: %d, Left: %d, %c", encoder_read(RIGHT_ENCODER), encoder_read(LEFT_ENCODER), 3);
if (stop_press()) {
motor_set_vel(LEFT_MOTOR, 0);
motor_set_vel(RIGHT_MOTOR, 0);
encoder_reset(RIGHT_ENCODER);
encoder_reset(LEFT_ENCODER);
printf("\nRight: %d, Left: %d, %c", encoder_read(RIGHT_ENCODER), encoder_read(LEFT_ENCODER), 3);
go_click();
// Poliwhirl~
}
pause(100);
}
return 0;
}
void setup_filter() {
if (stop_press()) {
state = STOP;
return;
}
state_time = get_time();
state = MOVING;
}
void moving_filter() {
if (stop_press()) {
state = STOP;
return;
}
uint16_t left_encoder_change = encoder_read(LEFT_ENCODER) - left_encoder_base;
uint16_t right_encoder_change = encoder_read(RIGHT_ENCODER) - right_encoder_base;
if ((left_encoder_change + right_encoder_change)/2 >= CM_TO_TICKS(SQUARE_LENGTH_CM)) {
target_angle += 90;
//target_angle = (int)target_angle%360;
state = TURNING;
soft_stop_motors(200);
}
}
void turning_filter() {
if (stop_press()) {
state = STOP;
return;
}
if (gyro_get_degrees() > target_angle - TURNING_THRESHOLD &&
gyro_get_degrees() < target_angle + TURNING_THRESHOLD) {
reset_pid_controller(target_angle);
state_time = get_time();
state = MOVING;
if (target_angle == 360) {
state = STOP;
return;
}
soft_stop_motors(500);
}
}
void test_motors() {
uint8_t mot;
uint16_t pos;
while(1){
printf("Please choose a motor to test: (0 or 1 please)\n");
scanf("%d", &mot);
if (mot >= 0 && mot <= 6){
printf("Push Go to start testing Motor %d. Push stop at any time to restart the test.\nNote that 0 on the knob corresponds to moving backwards at -255, 255 on knob corresponds to stopped, and 511 on knob corresponds to moving forwards at 255.\n\n", mot);
break;
}
else
printf("Not a valid motor. Please try again.\n\n");
}
go_click();
printf("Now Testing Motor %d", mot);
while (!stop_press()) {
pos = frob_read()/2;
printf("Motor %d is set to %3d with a current of %dmA\n",mot,pos,motor_get_current_MA(mot));
motor_set_vel(mot,pos-256);
pause(50);
}
motor_set_vel(mot,0);
}
