State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

Committer:
brass_phoenix
Date:
Thu Nov 01 15:41:34 2018 +0000
Revision:
39:f119ca6fc821
Parent:
33:543debddb3a9
Child:
51:e0e4d7e3de93
+ EMG possibly calibratable.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brass_phoenix 20:af1a6cd7469b 1 #pragma once
brass_phoenix 20:af1a6cd7469b 2
brass_phoenix 20:af1a6cd7469b 3 const double PI = 3.14159265359;
brass_phoenix 20:af1a6cd7469b 4 // Main loop wait time per cycle. This does not influence the motor PID or EMG reading frequencies.
brass_phoenix 20:af1a6cd7469b 5 const float main_loop_wait_time = 0.01;
brass_phoenix 20:af1a6cd7469b 6
brass_phoenix 20:af1a6cd7469b 7 // Time between two button polls. Used to debounce the button presses.
brass_phoenix 20:af1a6cd7469b 8 const float button_poll_interval = 0.05;
brass_phoenix 20:af1a6cd7469b 9
brass_phoenix 20:af1a6cd7469b 10 const float pid_period = 0.001; // PID sample period in seconds.
brass_phoenix 20:af1a6cd7469b 11
brass_phoenix 39:f119ca6fc821 12 const float emg_period = 0.005; // EMG sample period in seconds.
brass_phoenix 39:f119ca6fc821 13
brass_phoenix 20:af1a6cd7469b 14 const float Kp = 10.0;
brass_phoenix 20:af1a6cd7469b 15 const float Ki = 0.1;
brass_phoenix 20:af1a6cd7469b 16 const float Kd = 0.5;
brass_phoenix 20:af1a6cd7469b 17
brass_phoenix 20:af1a6cd7469b 18 // The motor -> main gear ratio is 25 / 60.
brass_phoenix 20:af1a6cd7469b 19 // The motor -> secondary gear ratio is 25/50
brass_phoenix 20:af1a6cd7469b 20 const float main_gear_ratio = 25.0/60.0;
brass_phoenix 20:af1a6cd7469b 21 const float sec_gear_ratio = 25.0/50.0;
brass_phoenix 20:af1a6cd7469b 22
brass_phoenix 33:543debddb3a9 23 const double main_motor_calibration_angle = 0.785398; // 45 degrees.
brass_phoenix 33:543debddb3a9 24 const double sec_motor_calibration_angle = -0.733038; // -42 degrees.
brass_phoenix 20:af1a6cd7469b 25
brass_phoenix 20:af1a6cd7469b 26 const float L0 = 0.15; // Length between two motors [meter]
brass_phoenix 20:af1a6cd7469b 27 const float L1 = 0.10; // Length first beam from right motor2 [meter]
brass_phoenix 20:af1a6cd7469b 28 const float L2 = 0.30; // Length second beam from right motor2 [meter]
brass_phoenix 20:af1a6cd7469b 29 const float L3 = 0.15; // Length beam between L2 and L4 [meter]
brass_phoenix 20:af1a6cd7469b 30 const float L4 = 0.30; // Length first beam from left motor1 [meter]
brass_phoenix 20:af1a6cd7469b 31 const float L5 = 0.35; // Length from L3 to end-effector [meter]
brass_phoenix 20:af1a6cd7469b 32 const float L6 = 1.0; // Length beam between frame 0 and motor 1 [meter]
brass_phoenix 20:af1a6cd7469b 33
brass_phoenix 27:b247e78a4380 34 // For inverse kinematics, maximum values for angles.
brass_phoenix 27:b247e78a4380 35 const float max_r_angle = 2.96706; // If delta is larger than 175 degrees r is larger than this
brass_phoenix 20:af1a6cd7469b 36 // Maximum and minimum angles of the main and secondary arm.
brass_phoenix 33:543debddb3a9 37 const float main_arm_max_angle = 2.79253;
brass_phoenix 33:543debddb3a9 38 const float main_arm_min_angle = main_motor_calibration_angle - 0.05;
brass_phoenix 20:af1a6cd7469b 39 const float sec_arm_max_angle = 1.48353;
brass_phoenix 33:543debddb3a9 40 const float sec_arm_min_angle = sec_motor_calibration_angle + 0.05;