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;
}
static int encoder_integrator_daemon() {
last_encs.l = encoder_read(PIN_ENCODER_WHEEL_L);
last_encs.r = encoder_read(PIN_ENCODER_WHEEL_R);
while(1) {
flush_integration();
pause(ENCODER_INTEGRATION_FREQUENCY_MS);
}
return 0;
}
void matrix_scan_quantum() {
#if defined(AUDIO_ENABLE) && !defined(NO_MUSIC_MODE)
matrix_scan_music();
#endif
#ifdef TAP_DANCE_ENABLE
matrix_scan_tap_dance();
#endif
#ifdef COMBO_ENABLE
matrix_scan_combo();
#endif
#if defined(BACKLIGHT_ENABLE) && defined(BACKLIGHT_PIN)
backlight_task();
#endif
#ifdef RGB_MATRIX_ENABLE
rgb_matrix_task();
if (rgb_matrix_task_counter == 0) {
rgb_matrix_update_pwm_buffers();
}
rgb_matrix_task_counter = ((rgb_matrix_task_counter + 1) % (RGB_MATRIX_SKIP_FRAMES + 1));
#endif
#ifdef ENCODER_ENABLE
encoder_read();
#endif
matrix_scan_kb();
}
uint8_t matrix_scan(void) {
uint8_t ret = _matrix_scan();
if (is_keyboard_master()) {
static uint8_t error_count;
if (!transport_master(matrix + thatHand)) {
error_count++;
if (error_count > ERROR_DISCONNECT_COUNT) {
// reset other half if disconnected
for (int i = 0; i < ROWS_PER_HAND; ++i) {
matrix[thatHand + i] = 0;
}
}
} else {
error_count = 0;
}
matrix_scan_quantum();
} else {
transport_slave(matrix + thisHand);
#ifdef ENCODER_ENABLE
encoder_read();
#endif
matrix_slave_scan_user();
}
return ret;
}
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);
}
}
/**
* Reads data from the encoder (as much as available) and returns it
* as a new #page object.
*/
static struct page *
httpd_output_read_page(struct httpd_output *httpd)
{
size_t size = 0, nbytes;
if (httpd->unflushed_input >= 65536) {
/* we have fed a lot of input into the encoder, but it
didn't give anything back yet - flush now to avoid
buffer underruns */
encoder_flush(httpd->encoder, NULL);
httpd->unflushed_input = 0;
}
do {
nbytes = encoder_read(httpd->encoder, httpd->buffer + size,
sizeof(httpd->buffer) - size);
if (nbytes == 0)
break;
httpd->unflushed_input = 0;
size += nbytes;
} while (size < sizeof(httpd->buffer));
if (size == 0)
return NULL;
return page_new_copy(httpd->buffer, size);
}
static void
encoder_to_stdout(struct encoder *encoder)
{
size_t length;
static char buffer[32768];
while ((length = encoder_read(encoder, buffer, sizeof(buffer))) > 0)
write(1, buffer, length);
}
void moving_state() {
printf("\nMoving state");
left_encoder_base = encoder_read(LEFT_ENCODER);
right_encoder_base = encoder_read(RIGHT_ENCODER);
while(state == MOVING)
{
float input = gyro_get_degrees();
float output = update_pid_input(&controller, input);
motor_set_vel(RIGHT_MOTOR, FORWARD_SPEED + (int)output + OFFSET_ESTIMATE);
motor_set_vel(LEFT_MOTOR, FORWARD_SPEED - (int)output - OFFSET_ESTIMATE);
pause(50);
moving_filter();
}
}
void position_controller( char pos )
{
int16_t rotations = encoder_read();
int16_t prefered_rotations = ref_pos + ( ( (int32_t)pos ) * 8000.0) / 255;
int16_t error = -prefered_rotations + rotations;
char to_motor = error >> 6;
motor_drive(to_motor*2);
}
int umain (void) {
printf("testing wheels forward...\n");
set_wheel_pows(0.3, 0.3);
pause(50);
set_wheel_pows(0, 0);
printf("done...\n");
printf("preparing to print sensors...\n");
while(1) {
printf("gyro: %f", gyro_get_degrees());
printf(" enc_l: %i", encoder_read(PIN_ENCODER_WHEEL_L));
printf(" enc_r: %i", encoder_read(PIN_ENCODER_WHEEL_R));
printf("\n");
pause(50);
}
return 0;
}
int umain (void) {
printf("print encs_snapshot\n");
encoder_reset(PIN_ENCODER_WHEEL_L);
encoder_reset(PIN_ENCODER_WHEEL_R);
printf("encs_snapshot [%i : %i]\n", encoder_read(PIN_ENCODER_WHEEL_L), encoder_read(PIN_ENCODER_WHEEL_R));
printf("move seq\n");
set_wheel_pows(0.4, 0.4);
while(!are_we_there_yet()) {}
printf("stopping...");
set_wheel_pows(0, 0);
printf(" done\n");
printf("encs_snapshot [%i : %i]\n", encoder_read(PIN_ENCODER_WHEEL_L), encoder_read(PIN_ENCODER_WHEEL_R));
return 0;
}
int main(int argc, char *argv[]) {
board_t *board = NULL;
int ret = 0;
encoder_module_t enc;
if (argc == 1) {
print_short_usage();
return 0;
}
if (process_cmd_line(argc, argv) == -1) {
return -1;
}
board_access.verbose = verbose_flag;
if (anyio_init(&board_access) != 0) { // init library
return -1;
}
ret = anyio_find_dev(&board_access); // find board
if (ret < 0) {
return -1;
}
board = anyio_get_dev(&board_access, 1); // if found the get board handle
if (board == NULL) {
printf("No %s board found\n", board_access.device_name);
return -1;
}
board->open(board); // open board for communication
board->print_info(board); // print what card it is
hm2_read_idrom(&(board->llio.hm2)); // read hostmot2 idrom
ret = encoder_init(&enc, board, instance, delay); // init encoder 'instance' module on 'board'
if (ret < 0) {
goto fail0;
}
while (1) {
encoder_read(&enc); // read encoder
printf("tsc = %u, raw_counts = %u, velocity = %.2f\n", enc.global_time_stamp, enc.raw_counts, enc.velocity);
usleep(delay*1000); // wait delay ms
}
encoder_cleanup(&enc); // cleanup enocder module
fail0:
board->close(board); // close board communication
anyio_cleanup(&board_access); // close library
return 0;
}
static void flush_integration() {
// printf("encoder_integrator::flush_integration()\n");
l_r_float_t delta_encs = {
(get_wheel_pows().l > 0 ? 1 : -1) * (encoder_read(PIN_ENCODER_WHEEL_L) - last_encs.l) ,
(get_wheel_pows().r > 0 ? 1 : -1) * (encoder_read(PIN_ENCODER_WHEEL_R) - last_encs.r) };
// printf("delta encs == [%f, %f]\n", delta_encs.l, delta_encs.r);
acquire(&encoder_integrator_daemon_lock);
float avg_delta_encs = (delta_encs.l + delta_encs.r) / 2;
float distance_traveled_fd_axis = avg_delta_encs * MM_PER_TICK_WHEELS;
guess_pos.x += distance_traveled_fd_axis * cos(guess_theta * DEGS_TO_RADS);
guess_pos.y += distance_traveled_fd_axis * sin(guess_theta * DEGS_TO_RADS);
float diff_delta_encs = delta_encs.r - delta_encs.l;
guess_theta += (diff_delta_encs * MM_PER_TICK_WHEELS) / (M_PI * MM_WHEEL_FROM_CENTER) * 90;
release(&encoder_integrator_daemon_lock);
last_encs.l = encoder_read(PIN_ENCODER_WHEEL_L);
last_encs.r = encoder_read(PIN_ENCODER_WHEEL_R);
}
/**
* Reads data from the encoder (as much as available) and returns it
* as a new #page object.
*/
static struct page *
httpd_output_read_page(struct httpd_output *httpd)
{
size_t size = 0, nbytes;
do {
nbytes = encoder_read(httpd->encoder, httpd->buffer + size,
sizeof(httpd->buffer) - size);
if (nbytes == 0)
break;
size += nbytes;
} while (size < sizeof(httpd->buffer));
if (size == 0)
return NULL;
return page_new_copy(httpd->buffer, size);
}
void motor_init(void)
{
i2c_init();
DDRF = 0xFF; // MJ1 output
DDRK = 0x00;
PORTF |= (1 << PF4); // Enable motor
//Move to known reference point
motor_drive(-50);
_delay_ms(1000);
motor_drive(0);
//Toggle reset encoder
PORTF &= ~(1 << PF6);
_delay_us(5);
PORTF |= (1 << PF6);
ref_pos = encoder_read();
}
/**
* Writes pending data from the encoder to the output file.
*/
static bool
recorder_output_encoder_to_file(struct recorder_output *recorder,
GError **error_r)
{
size_t size = 0, position, nbytes;
assert(recorder->fd >= 0);
/* read from the encoder */
size = encoder_read(recorder->encoder, recorder->buffer,
sizeof(recorder->buffer));
if (size == 0)
return true;
/* write everything into the file */
position = 0;
while (true) {
nbytes = write(recorder->fd, recorder->buffer + position,
size - position);
if (nbytes > 0) {
position += (size_t)nbytes;
if (position >= size)
return true;
} else if (nbytes == 0) {
/* shouldn't happen for files */
g_set_error(error_r, recorder_output_quark(), 0,
"write() returned 0");
return false;
} else if (errno != EINTR) {
g_set_error(error_r, recorder_output_quark(), 0,
"Failed to write to '%s': %s",
recorder->path, g_strerror(errno));
return false;
}
}
}
void initialize(){
float skew = 0;//find_skew(field_state.curr_loc);
build_bisecting_points(skew, &bisecting_points);
build_lever_pivot_points(&lever_pivot_points);
build_territory_pivot_points(&territory_pivot_points);
field_state.stage = FIRST_STAGE;
field_state.drive_direction = BACKWARD;
field_state.substage = PIVOT_SUBSTAGE;
accelerate_time = DRIVE_ACCELERATE_TIME;
field_state.pid_enabled = TRUE;
decelerate_distance = CIRCLE_DECELERATE_DISTANCE_FIRST;
field_state.stored_time = get_time();
field_state.curr_time = get_time();
target_vel = TARGET_CIRCLE_VEL;
field_state.start_time = get_time();
uint8_t changed_last_update = FALSE;
field_state.encoder_value = encoder_read(FREEWHEEL_ENCODER_PORT);
field_state.balls_held = 0;
current_vel = 0;
update_field();
log_init(30000);
uint8_t sextant = get_current_sextant(field_state.curr_loc);
if(sextant == 0){
field_state.color = BLUE;
}
else if(sextant == 3){
field_state.color = RED;
}
else{
field_state.color = 2;
}
Location target_loc = get_territory_pivot_point_loc(mod_ui(sextant - 1, 6));
set_new_destination(field_state.curr_loc, target_loc);
update_field();
servo_set_pos(0, 300);
}
int main(void)
{
//Local variables:
uint8 i = 0;
unsigned char result = 0;
uint8 toggle_wdclk = 0;
uint8 cmd_ready_485_1 = 0, cmd_ready_usb = 0;
static uint8 new_cmd_led = 0;
uint16 safety_delay = 0;
uint8 i2c_time_share = 0;
//Power on delay with LEDs
power_on();
//Initialize all the peripherals
init_peripherals();
//Start with an empty buffer
flexsea_clear_slave_read_buffer();
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
//Blocking Test code - enable one and only one for special
//debugging. Normal code WILL NOT EXECUTE when this is enabled!
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
//strain_test_blocking();
//safety_cop_comm_test_blocking();
//imu_test_code_blocking();
//motor_fixed_pwm_test_code_blocking(141);
//wdclk_test_blocking();
//timing_test_blocking();
//test_current_tracking_blocking();
//test_pwm_pulse_blocking();
//test_uart_dma_xmit();
//motor_cancel_damping_test_code_blocking();
//csea_knee_up_down_test_demo();
//motor_stepper_test_blocking_1(80);
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
//Non-Blocking Test code
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
#ifdef USE_SPI_COMMUT
motor_stepper_test_init(0);
//Note: deadtime is 55, small PWM values won't make it move.
//Starting at 0, GUI will change that when it wants.
#endif //USE_SPI_COMMUT
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
//Special code for the ExoBoots:
#ifdef PROJECT_EXOCUTE
init_exo();
#endif
//Main loop
while(1)
{
if(t1_new_value == 1)
{
//If the time share slot changed we run the timing FSM. Refer to
//timing.xlsx for more details. 't1_new_value' updates at 10kHz,
//each slot at 1kHz.
t1_new_value = 0;
//Timing FSM:
switch(t1_time_share)
{
//Case 0: I2C
case 0:
i2c_time_share++;
i2c_time_share %= 4;
#ifdef USE_I2C_INT
//Subdivided in 4 slots.
switch(i2c_time_share)
{
//Case 0.0: Accelerometer
case 0:
#ifdef USE_IMU
get_accel_xyz();
i2c_last_request = I2C_RQ_ACCEL;
#endif //USE_IMU
break;
//Case 0.1: Gyroscope
case 1:
#ifdef USE_IMU
get_gyro_xyz();
i2c_last_request = I2C_RQ_GYRO;
#endif //USE_IMU
break;
//Case 0.2: Safety-Cop
case 2:
safety_cop_get_status();
i2c_last_request = I2C_RQ_SAFETY;
break;
//Case 0.3: Free
case 3:
//I2C RGB LED
//minm_test_code();
update_minm_rgb(); //ToDo: That's EXT_I2C, not INT
break;
default:
break;
}
#endif //USE_I2C_INT
#ifdef USE_SPI_COMMUT
angle = as5047_read_single(AS5047_REG_ANGLEUNC);
#endif //USE_SPI_COMMUT
break;
//Case 1:
case 1:
break;
//Case 2:
case 2:
break;
//Case 3: Strain Gauge DelSig ADC, SAR ADC
case 3:
//Start a new conversion
ADC_DelSig_1_StartConvert();
//Filter the previous results
strain_filter_dma();
break;
//Case 4: User Interface
case 4:
//Alive LED
alive_led();
//UI RGB LED:
if(safety_delay > SAFETY_DELAY)
{
//status_error_codes(safety_cop.status1, safety_cop.status2, &eL0, &eL1, &eL2);
}
else
{
safety_delay++;
}
//Display temperature status on RGB
overtemp_error(&eL1, &eL2); //Comment this line if safety code is problematic
rgb_led_ui(eL0, eL1, eL2, new_cmd_led); //ToDo add more error codes
if(new_cmd_led)
{
new_cmd_led = 0;
}
break;
//Case 5: Quadrature encoder & Position setpoint
case 5:
#ifdef USE_QEI1
//Refresh encoder readings
encoder_read();
#endif //USE_QEI1
#ifdef USE_TRAPEZ
if((ctrl.active_ctrl == CTRL_POSITION) || (ctrl.active_ctrl == CTRL_IMPEDANCE))
{
//Trapezoidal trajectories (can be used for both Position & Impedance)
ctrl.position.setp = trapez_get_pos(steps); //New setpoint
ctrl.impedance.setpoint_val = trapez_get_pos(steps); //New setpoint
}
#endif //USE_TRAPEZ
break;
//Case 6: P & Z controllers, 0 PWM
case 6:
#ifdef USE_TRAPEZ
if(ctrl.active_ctrl == CTRL_POSITION)
{
motor_position_pid(ctrl.position.setp, ctrl.position.pos);
}
else if(ctrl.active_ctrl == CTRL_IMPEDANCE)
{
//Impedance controller
motor_impedance_encoder(ctrl.impedance.setpoint_val, ctrl.impedance.actual_val);
}
#endif //USE_TRAPEZ
//If no controller is used the PWM should be 0:
if(ctrl.active_ctrl == CTRL_NONE)
{
motor_open_speed_1(0);
}
break;
case 7:
#ifdef USE_SPI_COMMUT
//Stepper test code:
motor_stepper_test_runtime(10);
#endif //USE_SPI_COMMUT
break;
//Case 8: SAR ADC filtering
case 8:
filter_sar_adc();
break;
//Case 9: User functions
case 9:
//ExoBoot code - 1kHz
#ifdef PROJECT_EXOCUTE
exo_fsm();
#endif
break;
default:
break;
}
//The code below is executed every 100us, after the previous slot.
//Keep it short!
//BEGIN - 10kHz Refresh
//RS-485 Byte Input
#ifdef USE_RS485
//get_uart_data(); //Now done via DMA
if(data_ready_485_1)
{
data_ready_485_1 = 0;
//Got new data in, try to decode
cmd_ready_485_1 = unpack_payload_485_1();
}
#endif //USE_RS485
//USB Byte Input
#ifdef USE_USB
get_usb_data();
if(data_ready_usb)
{
data_ready_usb = 0;
//Got new data in, try to decode
cmd_ready_usb = unpack_payload_usb();
eL1 = 1;
}
#endif //USE_USB
//FlexSEA Network Communication
#ifdef USE_COMM
//Valid communication from RS-485 #1?
if(cmd_ready_485_1 != 0)
{
cmd_ready_485_1 = 0;
//Cheap trick to get first line //ToDo: support more than 1
for(i = 0; i < PAYLOAD_BUF_LEN; i++)
{
tmp_rx_command_485_1[i] = rx_command_485_1[0][i];
}
//payload_parse_str() calls the functions (if valid)
result = payload_parse_str(tmp_rx_command_485_1);
//LED:
if(result == PARSE_SUCCESSFUL)
{
//Green LED only if the ID matched and the command was known
new_cmd_led = 1;
}
}
//Valid communication from USB?
if(cmd_ready_usb != 0)
{
cmd_ready_usb = 0;
//Cheap trick to get first line //ToDo: support more than 1
for(i = 0; i < PAYLOAD_BUF_LEN; i++)
{
tmp_rx_command_usb[i] = rx_command_usb[0][i];
}
//payload_parse_str() calls the functions (if valid)
result = payload_parse_str(tmp_rx_command_usb);
//LED:
if(result == PARSE_SUCCESSFUL)
{
//Green LED only if the ID matched and the command was known
new_cmd_led = 1;
}
}
#endif //USE_COMM
//END - 10kHz Refresh
}
else
{
//Asynchronous code goes here.
//WatchDog Clock (Safety-CoP)
toggle_wdclk ^= 1;
WDCLK_Write(toggle_wdclk);
}
}
}
l_r_uint16_t get_encoders() {
l_r_uint16_t encs;
encs.l = encoder_read(PIN_ENCODER_WHEEL_L);
encs.r = encoder_read(PIN_ENCODER_WHEEL_R);
return encs;
}
float read_rotational_encoding() {
return (encoder_read(PIN_ENCODER_WHEEL_L) + encoder_read(PIN_ENCODER_WHEEL_R)) * MM_PER_TICK_WHEELS;
}
bool are_we_there_yet() {
float enc_avg = (encoder_read(PIN_ENCODER_WHEEL_L) + encoder_read(PIN_ENCODER_WHEEL_R)) / 2;
return enc_avg > 304.8 / MM_PER_TICK_WHEELS;
}
