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