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); }