State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
constants.h
- Committer:
- brass_phoenix
- Date:
- 2018-11-01
- Revision:
- 39:f119ca6fc821
- Parent:
- 33:543debddb3a9
- Child:
- 51:e0e4d7e3de93
File content as of revision 39:f119ca6fc821:
#pragma once const double PI = 3.14159265359; // Main loop wait time per cycle. This does not influence the motor PID or EMG reading frequencies. const float main_loop_wait_time = 0.01; // Time between two button polls. Used to debounce the button presses. const float button_poll_interval = 0.05; const float pid_period = 0.001; // PID sample period in seconds. const float emg_period = 0.005; // EMG sample period in seconds. const float Kp = 10.0; const float Ki = 0.1; const float Kd = 0.5; // The motor -> main gear ratio is 25 / 60. // The motor -> secondary gear ratio is 25/50 const float main_gear_ratio = 25.0/60.0; const float sec_gear_ratio = 25.0/50.0; const double main_motor_calibration_angle = 0.785398; // 45 degrees. const double sec_motor_calibration_angle = -0.733038; // -42 degrees. const float L0 = 0.15; // Length between two motors [meter] const float L1 = 0.10; // Length first beam from right motor2 [meter] const float L2 = 0.30; // Length second beam from right motor2 [meter] const float L3 = 0.15; // Length beam between L2 and L4 [meter] const float L4 = 0.30; // Length first beam from left motor1 [meter] const float L5 = 0.35; // Length from L3 to end-effector [meter] const float L6 = 1.0; // Length beam between frame 0 and motor 1 [meter] // For inverse kinematics, maximum values for angles. const float max_r_angle = 2.96706; // If delta is larger than 175 degrees r is larger than this // Maximum and minimum angles of the main and secondary arm. const float main_arm_max_angle = 2.79253; const float main_arm_min_angle = main_motor_calibration_angle - 0.05; const float sec_arm_max_angle = 1.48353; const float sec_arm_min_angle = sec_motor_calibration_angle + 0.05;