diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..f992f79 Binary files /dev/null and b/.DS_Store differ diff --git a/teensy40_brushed_v2/.DS_Store b/teensy40_brushed_v2/.DS_Store new file mode 100644 index 0000000..aae3971 Binary files /dev/null and b/teensy40_brushed_v2/.DS_Store differ diff --git a/teensy40_brushed_v2/.gitignore b/teensy40_brushed_v2/.gitignore new file mode 100644 index 0000000..beaaf8a --- /dev/null +++ b/teensy40_brushed_v2/.gitignore @@ -0,0 +1,6 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch +.vscode/extensions.json diff --git a/teensy40_brushed_v2/.vscode/extensions.json b/teensy40_brushed_v2/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/teensy40_brushed_v2/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/teensy40_brushed_v2/.vscode/settings.json b/teensy40_brushed_v2/.vscode/settings.json new file mode 100644 index 0000000..7f71f22 --- /dev/null +++ b/teensy40_brushed_v2/.vscode/settings.json @@ -0,0 +1,50 @@ +{ + "files.associations": { + "memory": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "compare": "cpp", + "concepts": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "random": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "initializer_list": "cpp", + "iosfwd": "cpp", + "limits": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp", + "iostream": "cpp", + "istream": "cpp", + "cstring": "cpp" + } +} \ No newline at end of file diff --git a/teensy40_brushed_v2/include/README b/teensy40_brushed_v2/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/teensy40_brushed_v2/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/teensy40_brushed_v2/lib/Arm/driver_motor.cpp b/teensy40_brushed_v2/lib/Arm/driver_motor.cpp new file mode 100644 index 0000000..4060995 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/driver_motor.cpp @@ -0,0 +1,371 @@ +/** + * @file driver_motor.cpp + * @author Vincent Boucher, Steve Ding + * @version V1.0.0 + * @date 30-May-2023 + * @brief motor object + * + * @attention For prototype example only + * V1.0.0 - created + * + */ +#include "driver_motor.h" +#include "pid.cpp" + +/// @brief Initializes the motor object +/// @param direction Positive direction for motor +/// @param port Port used for motor connection +/// @param maxTorque Maximum torque motor can output +void driver_motor::initialize_motor(uint8_t direction_motor, uint8_t motor_pwn_pin, uint8_t motor_dir_pin, uint8_t motor_fault_pin, float max_torque_motor, float torque_constant_motor) +{ + _motor_max_torque = max_torque_motor; + _torque_constant_motor = torque_constant_motor; + _target_torque = 0.0; + _target_angle_ps = 0.0; // The desired angle to turn to (sent from software) + _output_motor = 0; + _gear_ratio = 1.0; // default for no effect + _angle_full_turn_es = 360.0f; // encoder angle corresponding to a full turn (considering gear ratio) + + // Linear joint by default. + _is_circular_joint = false; + // Foward direction logic level + _forward_dir = direction_motor; + + _ctrl_period = 0.0; + _sampling_period = 0.0; + + _motor_dir_pin = motor_dir_pin; + _motor_fault_pin = motor_fault_pin; + _motor_pwm_pin = motor_pwn_pin; + + // kalman params + _kalman_gain = 0; + _current_estimate = 0; + + _err_estimate = _ERR_ESTIMATE_INIT; + _last_estimate = _LAST_ESTIMATE_INIT; + _err_measure = _ERR_MEASURE_INIT; + _q = _Q_INIT; + + _error = 0.0; + _previous_error = 0.0; + + _error_int = 0.0; + _error_dir = 0.0; + + // initializes pins and resolution + // _pwm_set_resolution(_PWM_BIT_RESOLUTION); + + pinMode(_motor_pwm_pin, OUTPUT); + pinMode(_motor_dir_pin, OUTPUT); + pinMode(_motor_fault_pin, OUTPUT); + + // TODO: is this not necessary for pid? + // _pwm_setup(_motor_pwm_pin, _MAX_PWM_FREQUENCY); // Sets frequency of pwm + + // 24V motor therefore max is 24 and min is -24 + pid_instance = new PID(0.1, 24, -24, 2.0, 0, 0); +} + +/// @brief Sets the torque and writes the PWM value to the motor +/// @param torque Torque value to be written to the motor +void driver_motor::set_target_torque(float torque) +{ + _target_torque = torque; +} + +float driver_motor::get_target_torque(void) +{ + return _target_torque; +} + +float driver_motor::get_output_motor(void) +{ + return _output_motor; +} + +void driver_motor::set_target_angle_ps(float angle_ps) +{ + if (!_is_circular_joint) + { + if (angle_ps > _max_angle_ps) + { + _target_angle_ps = _max_angle_ps; + } + else if (angle_ps < _min_angle_ps) + { + _target_angle_ps = _min_angle_ps; + } + else + { + _target_angle_ps = angle_ps; + } + } + else + { + _target_angle_ps = angle_ps; + } + return; +} + +float driver_motor::get_target_angle_ps(void) +{ + return _target_angle_ps; +} + +void driver_motor::set_control_period(float period) +{ + _ctrl_period = period; + _sampling_period = _ctrl_period * 1e-6; +} + +float driver_motor::get_current_angle_es() +{ + this->_encoder->poll_encoder_angle(); + _current_angle_es = this->_encoder->get_angle_multi(); + return _current_angle_es; +} + +float driver_motor::get_current_angle_ps() +{ + this->_encoder->poll_encoder_angle(); + float temp = this->_encoder->get_angle_multi(); + if (_is_circular_joint) + { + temp = fmod(temp, _angle_full_turn_es); + } + return temp / _gear_ratio; +} + +void driver_motor::closed_loop_control_tick() +{ + // IMPORTANT: es means encoder space + // setpoint_es is the desired angle after gear ratio translation + float setpoint_es = _target_angle_ps * _gear_ratio; + + // angle_multi_es is the current angle after gear ratio translation + float angle_multi_es = get_current_angle_es(); + + // For later, diff can influence PID coefficients + float diff = setpoint_es - angle_multi_es; + + // Determine whether to move forward or backwards + float forward_distance = (setpoint_es > angle_multi_es) ? (setpoint_es - angle_multi_es) : (_angle_full_turn_es - (angle_multi_es - setpoint_es)); // CCW + float backward_distance = (setpoint_es > angle_multi_es) ? (_angle_full_turn_es - (setpoint_es - angle_multi_es)) : (angle_multi_es - setpoint_es); // CW + + // CAPPING ANGLE BY JOINT LIMITS, only for linear joints + if (!_is_circular_joint) + { + if (setpoint_es > _max_angle_es) + { + setpoint_es = _max_angle_es; + } + else if (setpoint_es < _min_angle_es) + { + setpoint_es = _min_angle_es; + } + } + + // Feed to PID and determine error + float pid_output = 0.0; + if (_is_circular_joint) + { + // Backwards + if (backward_distance < forward_distance - 10.0) // TODO: handle hysterics? 10.0 was used previously + { + // set_direction(!_forward_dir); // set direction to backwards + if (setpoint_es < angle_multi_es) + { + pid_output = pid_instance->calculate(setpoint_es, angle_multi_es); + } + else + { + pid_output = pid_instance->calculate(setpoint_es + _angle_full_turn_es, angle_multi_es); + } + } + // Forwards + else + { + // set_direction(_forward_dir); // set direction to forwards + if (setpoint_es > angle_multi_es) + { + pid_output = pid_instance->calculate(setpoint_es, angle_multi_es); + } + else + { + pid_output = pid_instance->calculate(setpoint_es - _angle_full_turn_es, angle_multi_es); + } + } + } + else // linear joint + { + pid_output = pid_instance->calculate(setpoint_es, angle_multi_es); + } + + // Set direction + if (pid_output > 0) // pos + { + set_direction(_forward_dir); // set direction to forwards + } + else // neg + { + set_direction(!_forward_dir); // set direction to backwards + } + + // Output to motor + // 255 is the max PWM value, 24 is the max voltage + float pwm_output = abs(pid_output) * 255.0 / 24.0; + + this->_pwm_write_duty(pwm_output); + return; +} + +void driver_motor::torque_control(float motor_cur) +{ + + bool direction = LOW; + float motor_current; + + if (_forward_dir) + { + motor_current = motor_cur; + } + else + { + motor_current = -1 * motor_cur; + } + + _kalman_gain = _err_estimate / (_err_estimate + _err_measure); + _current_estimate = _last_estimate + _kalman_gain * (motor_current - _last_estimate); + + _err_estimate = (1.0 - _kalman_gain) * _err_estimate + fabs(_last_estimate - _current_estimate) * _q; + _last_estimate = _current_estimate; + + _error = _target_torque - _current_estimate * _torque_constant_motor; + _error_int += _error * _sampling_period; + + // Bound the integral error such that it cannot saturate the motors + // The typical error is less than 1 + if (_error_int > _maxError) + { + _error_int = _maxError; + } + else if (_error_int < -_maxError) + { + _error_int = -_maxError; + } + + // Calculate output + _output_motor = _error * _KP + _error_int * _KI + _error_dir * _KD; + + if (_output_motor == _output_motor) // TODO: why are we comparing it to itself? A: Its broken + { + _output_motor *= _PWM_OUTPUT_RESOLUTION / _motor_max_torque; + } + else + { + _output_motor = 0.0; + + _error = 0.0; + _previous_error = 0.0; + + _error_int = 0.0; + _error_dir = 0.0; + } + + // Set direction + if (_forward_dir == 0) + { + direction = !direction; + } + + if (_output_motor <= 0) + { + digitalWrite(_motor_dir_pin, direction); + } + else + { + digitalWrite(_motor_dir_pin, !direction); + } + + // output torque + _output_motor = abs(_output_motor); + if (_output_motor > _PWM_OUTPUT_RESOLUTION) + { + _pwm_write_duty(_PWM_OUTPUT_RESOLUTION); + } + else + { + _pwm_write_duty((uint32_t)_output_motor); + } +} + +/// @brief Sets the resolution of the motor +/// @param resolution new resolution of the motor +void driver_motor::_pwm_set_resolution(uint16_t resolution) +{ + + analogWriteResolution(resolution); +} + +/// @brief Sets the PWM frequency of the motor +/// @param pwmPin Pin used for PWM modulation to the motor +/// @param pwmFreq Frequency to be set for PWM +void driver_motor::_pwm_setup(float pwmFreq) +{ + analogWriteFrequency(_motor_pwm_pin, pwmFreq); +} + +/// @brief Writes the PWM value to the motor +/// @param pwmPin Pin used for PWM modulation to the motor +/// @param pwmDuty Duty cyle value to be written to PWM +void driver_motor::_pwm_write_duty(uint32_t pwmDuty) +{ + analogWrite(_motor_pwm_pin, pwmDuty); +} + +void driver_motor::set_gear_ratio(float gear_ratio) +{ + _gear_ratio = gear_ratio; + _angle_full_turn_es = 360.0f * _gear_ratio; +} + +void driver_motor::set_is_circular_joint(bool is_circular_joint) +{ + _is_circular_joint = is_circular_joint; +} + +/// @brief Define the forward logic level +/// @param forward_dir +void driver_motor::set_forward_dir(uint8_t forward_dir) +{ + _forward_dir = forward_dir; +} + +void driver_motor::set_angle_limit_ps(float max_angle, float min_angle) +{ + _max_angle_ps = max_angle; + _min_angle_ps = min_angle; + _max_angle_es = max_angle * _gear_ratio; + _min_angle_es = min_angle * _gear_ratio; +} + +void driver_motor::set_direction(uint8_t direction) +{ + digitalWrite(_motor_dir_pin, direction); +} + +void driver_motor::move_manual(uint32_t speed) +{ + uint32_t duty = abs(speed) * 255; + if (speed < 0) + { + digitalWrite(_motor_dir_pin, !_forward_dir); + _pwm_write_duty(duty); + } + else + { + digitalWrite(_motor_dir_pin, _forward_dir); + _pwm_write_duty(duty); + } +} diff --git a/teensy40_brushed_v2/lib/Arm/driver_motor.h b/teensy40_brushed_v2/lib/Arm/driver_motor.h new file mode 100644 index 0000000..649f8a8 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/driver_motor.h @@ -0,0 +1,207 @@ +/** + * @file driver_actuators.h + * @author Steve Ding, Oliver Philbin-Briscoe, Maciej Lacki, Colin Gallacher + * @version V0.2.0 + * @date 22-February-2022 + * @brief single actuator controller + * + * @attention For prototype example only + */ + +#ifndef DRIVER_JOINT_H +#define DRIVER_JOINT_H + +#include "Arduino.h" +#include "hardware_pins.h" + +#include "model_encoder.h" +#include "model_sensor.h" + +#include "pid.h" + +#include + +#define _MAX_PWM_FREQUENCY 146484.38 +#define _PWM_BIT_RESOLUTION 10 +#define _PWM_OUTPUT_RESOLUTION 1023 + +class driver_motor +{ +public: + /** + * Initialize a actuator control pins based on port and control parameters + * + * @param direction positive actuator rotation direction + * @param port actuator connection port + * @param maxTorque maximum torque limit actuator can output + */ + void initialize_motor(uint8_t directionMotor, uint8_t motorPWMPin, uint8_t motorDirPin, uint8_t motorFaultPin, float maxTorqueMotor, float torqueConstantMotor); + + /** + * Set specific torque target torque + * + * @param torque torque to be output + */ + void set_target_torque(float torque); + + void set_target_angle_ps(float position); + + void set_target_speed(float speed); + + void set_gear_ratio(float gear_ratio); + + void set_is_circular_joint(boolean is_circular_joint); + // Set the logic that defines the forward direction of the motor + void set_forward_dir(uint8_t forward_dir); + + /// @brief Sets the positive direction of the motor + /// @param direction Positive direction of motor (CW or CCW) + void set_direction(uint8_t direction); + + /// @brief Sets the control period of the motor PID loop + /// @param period + void set_control_period(float period); + + float get_target_torque(void); + + float get_output_motor(void); + + float get_target_angle_ps(void); + + float get_target_speed(void); + /** + * Control torque output + * + * @param mtrCurrent detected motor current at time index + * @param powerState main power state + */ + void torque_control(float motorCur); + + void position_control(float motorPos); + + void speed_control(float motorSpeed); + + /** + * Set hardware PWM duty cycle + * + * @param pwmDuty bit resolution value of desired PWM duty cycle + */ + void _pwm_write_duty(uint32_t pwmDuty); + + void attach_encoder(std::unique_ptr encoder) + { + _encoder = std::move(encoder); + } + + void attach_current_sensor(std::unique_ptr current_sensor) + { + _current_sensor = std::move(current_sensor); + } + + void closed_loop_control_tick(); + + void set_angle_limit_ps(float max_angle, float min_angle); + + // private: + // model_encoder *_encoder = nullptr; + // model_sensor *_current_sensor = nullptr; + std::unique_ptr _encoder; + std::unique_ptr _current_sensor; + + // JOINT CONFIG. + uint8_t _forward_dir; + float _gear_ratio; + float _angle_full_turn_es; + boolean _is_circular_joint; + float _max_angle_ps; + float _min_angle_ps; + float _max_angle_es; + float _min_angle_es; + + // JOINT STATE + float _current_angle_es; + float _current_angle_ps; + + uint8_t _motor_pwm_pin; + uint8_t _motor_dir_pin; + uint8_t _motor_fault_pin; + + float _motor_max_torque; + float _motor_max_speed; + float _motor_max_position; + float _motor_min_position; + + float _target_torque; + float _target_angle_ps; + float _targetSpeed; + + // PID parameters + PID *pid_instance; + float _output_motor; + float _ctrl_period; + float _sampling_period; + + float _torque_constant_motor; + + // TODO: Where are these values from? Are they the aggressive ones or normal ones? + const float _pi = 3.14159265358979; + const float _KP = 4.0; + const float _KI = 2.0; + const float _KD = 1.0; + + // Filter params + // kalman filter parameters + + const float _ERR_ESTIMATE_INIT = 1.877e-3; + const float _LAST_ESTIMATE_INIT = 0; + const float _ERR_MEASURE_INIT = 1.877e-3; + const float _Q_INIT = 0.0044; + + float _err_estimate; + float _last_estimate; + float _err_measure; + float _q; + + float _kalman_gain; + float _current_estimate; + + // Error parameters and variables + const float _maxError = 2.0; // 10000.0; //2.0; + + float _error; + float _previous_error; + + float _error_int; + float _error_dir; + + // const float _MAX_PWM_FREQUENCY = 146484.38; + // const uint8_t _PWM_BIT_RESOLUTION = 10; + // const uint32_t _PWM_OUTPUT_RESOLUTION = 1023; + + /** + * Set PWM signal bit resolution + * + * @param resolution new PWM bit resolution + */ + void _pwm_set_resolution(uint16_t resolution); + + /** + * Hardware PWM pin setup function + * + * @param pwmPin hardware PWM pin to setup + * @param pwmFreq desired hardware PWM frequency + */ + void _pwm_setup(float pwmFreq); + + /** + * Manually move the motor forwards or backwards + * + * @param speed in range -1 to 1 + */ + void move_manual(uint32_t speed); + + float get_current_angle_es(); + float get_current_angle_ps(); +}; + +#endif \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/hardware_pins.h b/teensy40_brushed_v2/lib/Arm/hardware_pins.h new file mode 100644 index 0000000..1d68d3a --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/hardware_pins.h @@ -0,0 +1,53 @@ +/** + * @file hardware_pins.h + * @author Steve Ding, Oliver Philbin-Briscoe, Colin Gallacher + * @version V0.1.0 + * @date 11-March-2021 + * @brief hardware pin definitions + * + * @attention For prototype example only + */ + +#ifndef HARDWARE_PINS_H +#define HARDWARE_PINS_H + +#include + +const uint8_t ENCPIN1_1 = 0; +const uint8_t ENCPIN1_2 = 1; + +const uint8_t ENCPIN2_1 = 2; +const uint8_t ENCPIN2_2 = 3; + +const uint8_t ENCPIN3_1 = 4; +const uint8_t ENCPIN3_2 = 5; + +const uint8_t PWMPIN1 = 13; // enable pin +const uint8_t DIRPIN1 = 16; // phase pin +const uint8_t nSLEEP1 = -1; + +const uint8_t PWMPIN2 = 14; +const uint8_t DIRPIN2 = 17; +const uint8_t nSLEEP2 = -1; + +const uint8_t PWMPIN3 = 15; +const uint8_t DIRPIN3 = 18; +const uint8_t nSLEEP3 = -1; + +const uint8_t LED_O = -1; +const uint8_t LED_B = -1; + +const uint8_t CURRENT_SENSE_A = 19; +const uint8_t CURRENT_SENSE_B = 20; +const uint8_t CURRENT_SENSE_C = 21; + +const uint8_t LIM_1 = 9; +const uint8_t LIM_2 = 10; +const uint8_t LIM_3 = 11; +const uint8_t LIM_4 = 12; +const uint8_t LIM_5 = 22; +const uint8_t LIM_6 = 23; + +const float wrist_pitch_max_angle = 30.0; +const float wrist_pitch_min_angle = -30.0; +#endif \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/model_encoder.cpp b/teensy40_brushed_v2/lib/Arm/model_encoder.cpp new file mode 100644 index 0000000..777a528 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/model_encoder.cpp @@ -0,0 +1,135 @@ +/** + * @file model_encoder.cpp + * @author Vincent Boucher, Steve Ding + * @version V1.0.0 + * @date 30-May-2023 + * @brief encoder object + * + * @attention For prototype example only + * V1.0.0 - created + * + */ +#include +#include "model_encoder.h" + +void model_encoder::initialize_encoder(uint8_t rotationalDirection, float offset, float resolution, uint8_t port) +{ + _offset = offset; + _resolution = resolution; + _port = port; + + _angularVelocity = 0.0; + _quad_enc_pos = _offset * (_resolution / 360.0); + _last_angle = 0.0; + + // TODO: check implementation multi-turn, should default true or false? + _turn_count = 0; + _angle_multi = 0.0; // angle from 0 to 360 in degrees + _angle_single = 0.0; // angle from 0 to 360 in degrees + _is_multi_turn = true; + + switch (port) + { + case 1: + _pinA = ENCPIN1_1; + _pinB = ENCPIN1_2; + break; + case 2: + _pinA = ENCPIN2_1; + _pinB = ENCPIN2_2; + break; + case 3: + _pinA = ENCPIN3_1; + _pinB = ENCPIN3_2; + break; + default: + _pinA = 0; + _pinB = 0; + break; + } + + if (rotationalDirection) + { + uint8_t tempPin = _pinA; + _pinA = _pinB; + _pinB = tempPin; + } + + _encoder = new QuadEncoder(port, _pinA, _pinB); + + _encoder->setInitConfig(); + _encoder->EncConfig.positionInitialValue = _quad_enc_pos; + _encoder->EncConfig.revolutionCountCondition = 1; + _encoder->init(); + + _velocityEstimation.initialize_parameters(_resolution, 8.0, 1000000.0, 1, 200000); +} + +void model_encoder::reset_encoder() +{ + _quad_enc_pos = _offset * (_resolution / 360.0); + _encoder->write(_quad_enc_pos); + _encoder->init(); +} + +void model_encoder::set_current_angle_es(float current_angle) +{ + Serial.printf("Setting as current: %f\n", current_angle); + _offset = current_angle; + _quad_enc_pos = current_angle * (_resolution / 360.0); + _encoder->write(_quad_enc_pos); +} + +void model_encoder::poll_encoder_angle() +{ + _quad_enc_pos = _encoder->read(); + + // Useless since _position always positive? + _angle_single = (_quad_enc_pos >= 0) ? (_quad_enc_pos % _resolution) : _resolution - (abs(_quad_enc_pos) % _resolution); + _angle_single = (360.0 / _resolution) * _angle_single; + _angle_multi = _quad_enc_pos * (360.0 / _resolution); + + _velocityEstimation.update_readings(_quad_enc_pos * (360.0 / _resolution), micros()); +} + +float model_encoder::get_angle_multi() +{ + return _angle_multi; +} + +float model_encoder::get_angle_single() +{ + return _angle_single; +} + +void model_encoder::set_parameters(uint8_t direction, float offset, float resolution) +{ + if (direction) + { + uint8_t tempPin = _pinA; + _pinA = _pinB; + _pinB = tempPin; + } + _encoder->enc_xbara_mapping(_pinA, PHASEA, 0); + _encoder->enc_xbara_mapping(_pinB, PHASEB, 0); + _encoder->disableInterrupts(_positionROEnable); + _encoder->disableInterrupts(_positionRUEnable); + + _offset = offset; + _resolution = resolution; + _quad_enc_pos = _offset * (_resolution / 360.0); + + _encoder->setInitConfig(); + _encoder->EncConfig.positionInitialValue = _quad_enc_pos; + _encoder->init(); +} + +void model_encoder::velocityEstimation(void) +{ + _angularVelocity = _velocityEstimation.foaw(0); +} + +float model_encoder::getVelocity(void) +{ + return _angularVelocity; +} \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/model_encoder.h b/teensy40_brushed_v2/lib/Arm/model_encoder.h new file mode 100644 index 0000000..b4b07f5 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/model_encoder.h @@ -0,0 +1,75 @@ +/** + * @file model_sensor_sensors.h + * @author Steve Ding, Oliver Philbin-Briscoe, Colin Gallacher + * @version V1.1.0 + * @date 26-Janurary-2023 + * @brief generic sensor object + * + * @attention For prototype example only + * V1.1.0 - added offset calculations for current and inkwell sensors + * + */ + +#ifndef MODEL_ENCODER_H +#define MODEL_ENCODER_H + +#include "hardware_pins.h" +#include "velocity_estimation.h" +#include "QuadEncoder.h" + +class model_encoder +{ +public: + QuadEncoder *_encoder; + + void initialize_encoder(uint8_t rotationalDirection, float offset, float resolution, uint8_t port); + + void reset_encoder(void); + + void set_current_angle_es(float offset); + + void poll_encoder_angle(void); + + float get_angle_multi(void); + + float get_angle_single(void); + + float get_full_angle_from_tick() + { + uint32_t tick = _encoder->read(); + return (float)tick * 360.0 / (float)_resolution; + } + + void set_parameters(uint8_t direction, float offset, float resolution); + + /** + * Main logic loop for calculating detected encoder velocity + * + * @param currentTime current micros count + */ + void velocityEstimation(void); + + float getVelocity(void); + +private: + // QuadEncoder *_encoder; + int32_t _offset; + int32_t _resolution; + uint8_t _port; + int32_t _quad_enc_pos; + uint8_t _pinA; + uint8_t _pinB; + + float _angle; + float _angle_single; + float _angle_multi; + float _angularVelocity; + velocity_estimation _velocityEstimation; + boolean _is_multi_turn; + int _turn_count; + float _last_angle; + // Offset compared to boot position + float _zero_angle_offset; +}; + +#endif \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/model_sensor.cpp b/teensy40_brushed_v2/lib/Arm/model_sensor.cpp new file mode 100644 index 0000000..a6fb875 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/model_sensor.cpp @@ -0,0 +1,94 @@ +/** + * @file model_sensor.cpp + * @author Vincent Boucher, Steve Ding + * @version V1.0.0 + * @date 30-May-2023 + * @brief current sensor object + * + * @attention For prototype example only + * V1.0.0 - created + * + */ +#include "model_sensor.h" + +void model_sensor::initialize_sensor(uint8_t pin) +{ + _sensor_value = 0; + _sensor_offset = 0; + + _pin = pin; + + _initialize_adc(); + + pinMode(pin, INPUT); + + delay(10); + + for (uint8_t i = 0; i < 40; i++) + { + _update_sensor_value(); + _sensor_offset = _sensor_offset + _sensor_value; + } + + _sensor_offset = _sensor_offset / 40; + + _sensor_offset = 1862 - _sensor_offset; +} + +void model_sensor::reset_sensor(void) +{ + _sensor_value = 0; +} + +float model_sensor::read_sensor_value(void) +{ + _update_sensor_value(); + float sensorValue = _sensor_value + _sensor_offset; + _current16 = sensorValue - 1862; + _raw_voltage = sensorValue * (3.3 / 4095.0); + _current = (_raw_voltage - 1.5) * 2; + return _current; +} + +float model_sensor::get_current(void) +{ + return _current; +} + +int16_t model_sensor::get_current_16(void) +{ + return _current16; +} + +float model_sensor::get_raw_voltage(void) +{ + return _raw_voltage; +} + +void model_sensor::_update_sensor_value(void) +{ + // if (_pin == CURRENT_SENSE_A1 || + // _pin == CURRENT_SENSE_B1 || + // _pin == CURRENT_SENSE_A2 || + // _pin == CURRENT_SENSE_B2) { + // _sensor_value = _adc->adc1->analogRead(_pin); + // }else{ + _sensor_value = _adc->adc0->analogRead(_pin); + // } +} + +void model_sensor::_initialize_adc(void) +{ + + _adc = new ADC(); + + _adc->adc0->setAveraging(4); + _adc->adc0->setResolution(12); + _adc->adc0->setConversionSpeed(ADC_CONVERSION_SPEED::VERY_HIGH_SPEED); + _adc->adc0->setSamplingSpeed(ADC_SAMPLING_SPEED::VERY_HIGH_SPEED); + + _adc->adc1->setAveraging(4); + _adc->adc1->setResolution(12); + _adc->adc1->setConversionSpeed(ADC_CONVERSION_SPEED::VERY_HIGH_SPEED); + _adc->adc1->setSamplingSpeed(ADC_SAMPLING_SPEED::VERY_HIGH_SPEED); +} \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/model_sensor.h b/teensy40_brushed_v2/lib/Arm/model_sensor.h new file mode 100644 index 0000000..d422c03 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/model_sensor.h @@ -0,0 +1,51 @@ +/** + * @file model_sensor_sensors.h + * @author Steve Ding, Oliver Philbin-Briscoe, Colin Gallacher + * @version V1.1.0 + * @date 26-Janurary-2023 + * @brief generic sensor object + * + * @attention For prototype example only + * V1.1.0 - added offset calculations for current and inkwell sensors + * + */ + +#ifndef MODEL_SENSOR_H +#define MODEL_SENSOR_H + +#include "Arduino.h" +#include +#include "hardware_pins.h" + +class model_sensor +{ +public: + void initialize_sensor(uint8_t pin); + + void reset_sensor(void); + + float read_sensor_value(void); + + float get_current(void); + + int16_t get_current_16(void); + + float get_raw_voltage(void); + +private: + ADC *_adc; + + int32_t _sensor_value; + uint8_t _pin; + float _current; + float _raw_voltage; + int16_t _current16; + + int32_t _sensor_offset; + + void _initialize_adc(void); + + void _update_sensor_value(void); +}; + +#endif \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/motor.h b/teensy40_brushed_v2/lib/Arm/motor.h new file mode 100644 index 0000000..e00e3ef --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/motor.h @@ -0,0 +1,366 @@ +#pragma once +#include "rotation2d.h" +#include "vector2d.h" +#include +/** + * definition of "In the x,y plane of the robot" : + * Where y is the vertical axis centered at the shoulder joint and x is parallel to the lateral movement of the arm. + */ + +/** + * Struct that stores the model of the motor. + * @author David Ly + */ +struct MotorModel +{ +public: + double stallCurrent, stallTorque, maxVoltage, maxRPM, resistance, Kt, Kv, gearRatio, freeCurrent; + /** + * Creates a new MotorModel with given parameters. + * @param a_stallCurrent The stall current of the motor in Amps. + * @param a_stallTorque The stall torque of the motor in Nm. + * @param a_maxVoltage The max voltage of the motor in Volts. + * @param a_maxRPM The max RPM of the motor in RPM. + * @param ratio The gear ratio of the motor. + */ + MotorModel(double a_stallCurrent, double a_stallTorque, double a_maxVoltage, double a_maxRPM, double a_freeCurrent, double ratio) + { + stallCurrent = a_stallCurrent; + stallTorque = a_stallTorque / ratio; + maxVoltage = a_maxVoltage; + maxRPM = a_maxRPM * ratio; + freeCurrent = a_freeCurrent; + gearRatio = ratio; + resistance = a_maxVoltage / a_stallCurrent; // based on ohm's law: in Ohms + Kt = stallTorque / stallCurrent; // Torque constant: Nm/A + Kv = maxRPM / (maxVoltage - freeCurrent * resistance); // Velocity constant: RPM/V + } + /** + * @return Velocity Constant in RPS/V + */ + double getKvRPS() + { + return Kv / 60.0; + } +}; +/** + * Struct that stores the state of the motor in terms of velocity and torque. + * @author David Ly + */ +struct MotorState +{ + /** + * Creates a new MotorState with given parameters. + * @param vel The velocity of the motor in RPS. + * @param torque The torque of the motor in Nm. + */ + MotorState(double vel, double torque) : vel(vel), + torque(torque) + { + } + double vel; + double torque; +}; +/** + * Struct that stores the pose of the arm in terms of the angles of the joints. + * @author David Ly + */ +struct ArmPose +{ + /** + * Creates a new ArmPose with given parameters. + * @param shoulder The angle of the shoulder joint. + * @param elbow The angle of the elbow joint. + * @param wrist The angle of the wrist joint. + */ + ArmPose(Rotation2d shoulder, Rotation2d elbow, Rotation2d wrist) : shoulder(shoulder), + elbow(elbow), + wrist(wrist) + { + } + Rotation2d shoulder; + Rotation2d elbow; + Rotation2d wrist; +}; +/** + * Defines the properties of a joint. + * @author David Ly + */ +struct JointProperties +{ +public: + const double mass, length; + const Vector2d cm_pos; + /** + * Creates a new JointProperties with given parameters. + * x, y position of the center of mass is relative to the center of rotation of the joint. + * Where x parallel to the longest side of the joint and y parallel to the shortest side of the joint. + * @param mass The mass of the joint in kg. + * @param cm_x The x position of the center of mass of the joint in m. + * @param cm_y The y position of the center of mass of the joint in m. + * @param length The length of the joint in m. + */ + JointProperties(double mass, double cm_x, double cm_y, double length) : mass(mass), + cm_pos(cm_x, cm_y), + length(length) + { + } + /** + * Creates a new JointProperties with given parameters. + * Center of mass position is relative to the center of rotation of the joint. + * Where x parallel to the longest side of the joint and y parallel to the shortest side of the joint. + * @param mass The mass of the joint in kg. + * @param cm The position of the center of mass of the joint in m. + * @param length The length of the joint in m. + */ + JointProperties(double mass, Vector2d cm, double length) : mass(mass), + cm_pos(cm.getX(), cm.getY()), + length(length) + { + } +}; + +#define GRAVITY 9.81 + +MotorModel shoulder_motor(111, 5.28, 24, 3210, 0.696, 120); +MotorModel elbow_motor(81.9, 2.49, 24, 4300, 0.497, 1); // todo: find ratio +MotorModel wrist_motor(24.5, GRAVITY * 90.0 / 100.0, 24, 13, 0.7, 1); // todo: find ratio + +// Distance of links in m, CM... +JointProperties shoulderProperties(1, 1, 1, 1); +JointProperties elbowProperties(1, 1, 1, 1); +JointProperties wristProperties(1, 1, 1, 1); + +#define ESTIMATED_HELDMASS_OFFSET 0.75 +/** + * Calculates the position of the end of the shoulder joint. + * In the x,y plane of the robot + * @param buf The vector to store the position of the end of the shoulder joint. + * @param armPose The pose of the arm. + */ +void getShoulderEndPosition(Vector2d *buf, ArmPose &armPose) +{ + Vector2d shoulderPositionVector(shoulderProperties.length, armPose.shoulder); + *buf += shoulderPositionVector; +} + +/** + * Calculates the position of the end of the elbow joint. + * In the x,y plane of the robot + * @param buf The vector to store the position of the end of the elbow joint. + * @param armPose The pose of the arm. + */ +void getElbowEndPosition(Vector2d *buf, ArmPose &armPose) +{ + getShoulderEndPosition(buf, armPose); + Rotation2d temp(armPose.shoulder.getRad() + armPose.elbow.getRad()); + Vector2d elbowPositionVector(elbowProperties.length, temp); + *buf += elbowPositionVector; +} + +/** + * Calculates the position of the end of the wrist joint. Asumming an offset for the point mass at the end of the joint + * In the x,y plane of the robot + * @param buf The vector to store the position of the end of the wrist joint. + * @param armPose The pose of the arm. + */ +void getWristEndPosition(Vector2d *buf, ArmPose &armPose) +{ + getElbowEndPosition(buf, armPose); + Rotation2d temp(armPose.shoulder.getRad() + armPose.elbow.getRad() + armPose.wrist.getRad()); + Vector2d wristPositionVector(wristProperties.length * ESTIMATED_HELDMASS_OFFSET, temp); + *buf += wristPositionVector; +} +/** + * Calculates the position of the center of mass of the held mass times the heldmass. + * In the x,y plane of the robot + * @param buf The vector to store the position of the center of mass of the held mass. + * @param armPose The pose of the arm. + * @param heldMass The mass of the held mass in kg. + */ +void calculateCenterMassofHeldMass(Vector2d *buf, ArmPose &armPose, double heldMass) +{ + getWristEndPosition(buf, armPose); + *buf *= heldMass; +} + +/** + * Calculates the center of mass * mass of the joint. + * In the x,y plane of the robot. + * @param buf OUT The vector to store the position of the center of mass * mass of the joint. + * @param jointPose The pose of the joint. + * @param properties The properties of the joint. + */ +void computeJointCM(Vector2d *buf, Rotation2d &jointPose, JointProperties &properties) +{ + // function assumes that buf is already filled with the value of the joint position in x,y + //(joint position is not hte same as joint cm) + Vector2d orientedJoint(0, 0); + orientedJoint += properties.cm_pos; // copy cm position vector + Vector2d::rotateBy(orientedJoint, jointPose); // orient cm position vector properly + *buf += orientedJoint; + *buf *= properties.mass; // cm * mass of the joint +} + +/** + * Calculates the torque on the wrist joint. + * In the x,y plane of the robot. + * @param armPose The pose of the arm. + * @param heldMass The mass of the held mass in kg. + * @return The torque on the wrist joint in Nm. + */ +double computeWristTorque(ArmPose &armPose, double heldMass) +{ + // Origin to CM of held mass + Vector2d heldMassPos(0, 0); // + calculateCenterMassofHeldMass(&heldMassPos, armPose, heldMass); + + // Origin to CM of wrist + Vector2d wristMass(0, 0); + getElbowEndPosition(&wristMass, armPose); + computeJointCM(&wristMass, armPose.wrist, wristProperties); + + double totalMass = wristProperties.mass + heldMass; + Vector2d centerMass(0, 0); + centerMass += wristMass; + centerMass += heldMassPos; + centerMass *= 1.0 / totalMass; + + Vector2d wristPosition(0, 0); + getElbowEndPosition(&wristPosition, armPose); + + // Wrist joint to new CM of wrist with held mass + Vector2d wristJointToCM(0, 0); + wristJointToCM += centerMass; + wristJointToCM -= wristPosition; + + // pendulum torque equation: torque = -mg/L sin(theta), theta is measured from the y-axis + // used cos, since angle is measured from the x-axis, so it would be sin(90deg-theta) = cos(theta) + double torque = -GRAVITY * totalMass / wristJointToCM.getLength() * wristJointToCM.getOrientation().getCos(); + return torque; +} + +/** + * Calculates the torque on the elbow joint. + * In the x,y plane of the robot. + * @param armPose The pose of the arm. + * @param heldMass The mass of the held mass in kg. + * @return The torque on the elbow joint in Nm. + */ +double computeElbowTorque(ArmPose &armPose, double heldMass) +{ + Vector2d heldMassPos(0, 0); + calculateCenterMassofHeldMass(&heldMassPos, armPose, heldMass); + + Vector2d wristMass(0, 0); + getElbowEndPosition(&wristMass, armPose); + computeJointCM(&wristMass, armPose.wrist, wristProperties); + + Vector2d elbowMass(0, 0); + getShoulderEndPosition(&elbowMass, armPose); + computeJointCM(&elbowMass, armPose.elbow, elbowProperties); + + double totalMass = elbowProperties.mass + wristProperties.mass + heldMass; + Vector2d centerMass(0, 0); + centerMass += wristMass; + centerMass += elbowMass; + centerMass += heldMassPos; + centerMass *= 1.0 / totalMass; + + Vector2d elbowPosition(0, 0); + getShoulderEndPosition(&elbowPosition, armPose); + + Vector2d elbowJointToCM(0, 0); + elbowJointToCM += centerMass; + elbowJointToCM -= elbowPosition; + + // pendulum torque equation: torque = -mg/L sin(theta), theta is measured from the y-axis + // used cos, since angle is measured from the x-axis, so it would be sin(90deg-theta) = cos(theta) + double torque = -GRAVITY * totalMass / elbowJointToCM.getLength() * elbowJointToCM.getOrientation().getCos(); + return torque; +} +/** + * Calculates the torque on the shoulder joint. + * In the x,y plane of the robot. + * @param armPose The pose of the arm. + * @param heldMass The mass of the held mass in kg. + * @return The torque on the shoulder joint in Nm. + */ +double computeShoulderTorque(ArmPose &armPose, double heldMass) +{ + Vector2d heldMassPos(0, 0); + calculateCenterMassofHeldMass(&heldMassPos, armPose, heldMass); + + Vector2d wristMass(0, 0); + getElbowEndPosition(&wristMass, armPose); + computeJointCM(&wristMass, armPose.wrist, wristProperties); + + Vector2d elbowMass(0, 0); + getShoulderEndPosition(&wristMass, armPose); + computeJointCM(&elbowMass, armPose.elbow, elbowProperties); + + Vector2d shoulderMass(0, 0); + computeJointCM(&shoulderMass, armPose.shoulder, shoulderProperties); + + double totalMass = shoulderProperties.mass + elbowProperties.mass + wristProperties.mass + heldMass; + Vector2d centerMass(0, 0); + centerMass += wristMass; + centerMass += elbowMass; + centerMass += shoulderMass; + centerMass += heldMassPos; + centerMass *= 1.0 / totalMass; + + Vector2d shoulderPosition(0, 0); + Vector2d shoulderJointToCM(0, 0); + shoulderJointToCM += centerMass; + shoulderJointToCM -= shoulderPosition; + + // pendulum torque equation: torque = -mg/L sin(theta), theta is measured from the y-axis + // used cos, since angle is measured from the x-axis, so it would be sin(90deg-theta) = cos(theta) + double torque = -GRAVITY * totalMass / shoulderJointToCM.getLength() * shoulderJointToCM.getOrientation().getCos(); + return torque; +} + +/** + * Calculates the voltage required to achieve the given state. + * @param requiredState The state to achieve. + * @param model The model of the motor. + */ +double findVoltage(MotorState &requiredState, MotorModel &model) +{ + return requiredState.torque / model.Kt * model.resistance + requiredState.vel * model.Kv; +} + +/** + * Calculates the state of the arm to maintain the given pose. + * @param pose Current arm pose. + * @param heldMass The mass of the held mass in kg. + */ +void maintainState(ArmPose &pose, double heldMass) +{ + MotorState wristMotorState(0, computeWristTorque(pose, heldMass)); + MotorState elbowMotorState(0, computeElbowTorque(pose, heldMass)); + MotorState shoulderMotorState(0, computeShoulderTorque(pose, heldMass)); + + double wristMotorVoltage = findVoltage(wristMotorState, wrist_motor); + double elbowMotorVoltage = findVoltage(elbowMotorState, elbow_motor); + double shoulderMotorVoltage = findVoltage(shoulderMotorState, shoulder_motor); +} + +JointProperties proto(.2, .07, 0, .1); + +double calculateTorque(const Rotation2d pos) +{ + Vector2d cm_pos(0, 0); + cm_pos += proto.cm_pos; + Vector2d::rotateBy(cm_pos, pos); + return -GRAVITY * proto.mass * cm_pos.getLength() * cm_pos.getOrientation().getSin(); // or getCos()? +} + +void maintainStateProto(Rotation2d pos, double *output) +{ + double torque = calculateTorque(pos); + MotorState motorState(0, torque); + double voltage = findVoltage(motorState, wrist_motor); + *output = voltage; +} \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/pid.cpp b/teensy40_brushed_v2/lib/Arm/pid.cpp new file mode 100644 index 0000000..082be05 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/pid.cpp @@ -0,0 +1,130 @@ +/** + * Copyright 2019 Bradley J. Snyder + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef _PID_SOURCE_ +#define _PID_SOURCE_ + +#include +#include +#include +#include "pid.h" + +using namespace std; + +class PIDImpl +{ +public: + PIDImpl(double dt, double max, double min, double Kp, double Kd, double Ki); + ~PIDImpl(); + double calculate(double setpoint, double pv); + void set_PID(double Kp, double Ki, double Kd); + +private: + double _dt; + double _max; + double _min; + double _Kp; + double _Kd; + double _Ki; + double _pre_error; + double _integral; +}; + +PID::PID(double dt, double max, double min, double Kp, double Kd, double Ki) +{ + pimpl = new PIDImpl(dt, max, min, Kp, Kd, Ki); +} +double PID::calculate(double setpoint, double pv) +{ + return pimpl->calculate(setpoint, pv); +} + +void PID::set_PID(double Kp, double Ki, double Kd) +{ + pimpl->set_PID(Kp, Ki, Kd); +} + +PID::~PID() +{ + delete pimpl; +} + +/** + * Implementation + */ +PIDImpl::PIDImpl(double dt, double max, double min, double Kp, double Kd, double Ki) : _dt(dt), + _max(max), + _min(min), + _Kp(Kp), + _Kd(Kd), + _Ki(Ki), + _pre_error(0), + _integral(0) +{ +} + +double PIDImpl::calculate(double setpoint, double pv) +{ + Serial.printf("setpoint: %f, pv: %f, ", setpoint, pv); + + // Calculate error + double error = setpoint - pv; + + // Proportional term + double Pout = _Kp * error; + + // Integral term + _integral += error * _dt; + double Iout = _Ki * _integral; + + // Derivative term + double derivative = (error - _pre_error) / _dt; + double Dout = _Kd * derivative; + + // Calculate total output + double output = Pout + Iout + Dout; + Serial.printf("Pout: %f, Iout: %f, Dout: %f, temp_output: %f, ", Pout, Iout, Dout, output); + + // Restrict to max/min + if (output > _max) + output = _max; + else if (output < _min) + output = _min; + + // Save error to previous error + _pre_error = error; + Serial.printf("output: %f\n", output); + return output; +} + +void PIDImpl::set_PID(double Kp, double Ki, double Kd) +{ + _Kp = Kp; + _Ki = Ki; + _Kd = Kd; +} + +PIDImpl::~PIDImpl() +{ +} + +#endif \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/pid.h b/teensy40_brushed_v2/lib/Arm/pid.h new file mode 100644 index 0000000..606c279 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/pid.h @@ -0,0 +1,47 @@ +/** + * Copyright 2019 Bradley J. Snyder + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef _PID_H_ +#define _PID_H_ + +class PIDImpl; +class PID +{ +public: + // Kp - proportional gain + // Ki - Integral gain + // Kd - derivative gain + // dt - loop interval time + // max - maximum value of manipulated variable + // min - minimum value of manipulated variable + PID(double dt, double max, double min, double Kp, double Kd, double Ki); + + // Returns the manipulated variable given a setpoint and current process value + double calculate(double setpoint, double pv); + ~PID(); + void set_PID(double Kp, double Ki, double Kd); + +private: + PIDImpl *pimpl; +}; + +#endif \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/rotation2d.h b/teensy40_brushed_v2/lib/Arm/rotation2d.h new file mode 100644 index 0000000..580b5b8 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/rotation2d.h @@ -0,0 +1,130 @@ +#pragma once +#include +#define PI 3.14159 +/** + * A simple class that represents a 2d Rotation regardless of unit. + * @author David Ly + */ +class Rotation2d +{ +private: + double _theta_rad, _sin, _cos; + +public: + /** + * Creates a new Rotation2d with given angle in radians. + * @param theta_rad The angle of the rotation in radians. + */ + explicit Rotation2d(double theta_rad) + { + _theta_rad = 0; + _sin = 0; + _cos = 0; + setAngleRad(theta_rad); + } + + /** + * Creates a new Rotation2d with given angle in degrees. + * @param theta_deg The angle of the rotation in degrees. + */ + static Rotation2d *getRotationFromDeg(double theta) + { + return new Rotation2d(theta / 180 * PI); + } + + /** + * Creates a new Rotation2d with given angle in rotations. + * @param theta The angle of the rotation in rotations. + */ + static Rotation2d *getRotationFromRotation(double theta) + { + return new Rotation2d(theta * PI); + } + + /** + * Sets the angle of the rotation in radians. + * @return The angle of the rotation in radians. + */ + void setAngleRad(double theta_rad) + { + _theta_rad = theta_rad; + _sin = sin(_theta_rad); + _cos = cos(_theta_rad); + } + + /** + * Gets the angle of the rotation in radians. + * @return The angle of the rotation in radians. + */ + double getRad() const + { + return _theta_rad; + } + + /** + * Gets the angle of the rotation in degrees. + * @return The angle of the rotation in degrees. + */ + double getDeg() const + { + return _theta_rad * 180 / PI; + } + + /** + * Gets the angle of the rotation in rotations. + * @return The angle of the rotation in rotations. + */ + double getRotation() const + { + return _theta_rad / (2 * PI); + } + + /** + * Gets the sin of the rotation. + * @return The sin of the rotation. + */ + double getSin() const { return _sin; } + + /** + * Gets the cos of the rotation. + * @return The cos of the rotation. + */ + double getCos() const { return _cos; } + + /** + * Adds the given rotation to this rotation. + * @param rot The rotation to add to this rotation. + */ + void operator+=(Rotation2d rot) + { + this->setAngleRad(this->_theta_rad + rot._theta_rad); + } + /** + * Subtracts the given rotation from this rotation. + * @param rot The rotation to subtract from this rotation. + */ + void operator-=(Rotation2d rot) + { + this->setAngleRad(this->_theta_rad - rot._theta_rad); + } + /** + * Adds the given rotation to this rotation. + * Returns a new object that's dynamically allocated + * @param rot The rotation to add to this rotation. + * @return The sum of the two rotations. + */ + Rotation2d *operator+(Rotation2d rot) const + { + return new Rotation2d(this->_theta_rad + rot._theta_rad); + } + /** + * Subtracts the given rotation from this rotation. + * Returns a new object that's dynamically allocated + * @param rot The rotation to subtract from this rotation. + * @return The difference of the two rotations. + */ + Rotation2d *operator-(Rotation2d rot) const + { + return new Rotation2d(this->_theta_rad - rot._theta_rad); + } +}; \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/vector2d.cpp b/teensy40_brushed_v2/lib/Arm/vector2d.cpp new file mode 100644 index 0000000..4c63865 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/vector2d.cpp @@ -0,0 +1,77 @@ +#pragma once +#include "vector2d.h" +#include +#define PI 3.14159 + +Vector2d::Vector2d(double x, double y) : _orientation(0), + _x(0), + _y(0), + _length(0) +{ + updateVectorfromCartesian(x, y); +} + +Vector2d::Vector2d(double length, Rotation2d orientation) : _orientation(0), + _x(0), + _y(0), + _length(0) +{ + updateVectorfromPolar(length, orientation); +} + +void Vector2d::updateVectorfromCartesian(double x, double y) +{ + _x = x; + _y = y; + _length = sqrt(x * x + y * y); + Rotation2d temp(atan2(y, x)); + _orientation += temp; +} +void Vector2d::updateVectorfromPolar(double l, Rotation2d orientation) +{ + _length = l; + _orientation.setAngleRad(orientation.getRad()); + _x = _length * _orientation.getCos(); + _y = _length * _orientation.getSin(); +} + +double Vector2d::getX() const { return _x; } +double Vector2d::getY() const { return _y; } +double Vector2d::getLength() const { return _length; } +Rotation2d Vector2d::getOrientation() const { return _orientation; } + +void Vector2d::operator*=(double scalar) +{ + this->updateVectorfromCartesian(_x * scalar, _y * scalar); +} + +void Vector2d::operator+=(Vector2d b) +{ + this->updateVectorfromCartesian(_x + b.getX(), _y + b.getY()); +} +void Vector2d::operator-=(Vector2d b) +{ + this->updateVectorfromCartesian(_x - b.getX(), _y - b.getY()); +} +Vector2d *Vector2d::operator*(double scalar) const +{ + return new Vector2d(scalar * _x, scalar * _y); +} +Vector2d *Vector2d::operator+(Vector2d b) const +{ + return new Vector2d(_x + b.getX(), _y + b.getY()); +} +Vector2d *Vector2d::operator-(Vector2d b) const +{ + return new Vector2d(_x - b.getX(), _y - b.getY()); +} + +double Vector2d::dot(Vector2d a, Vector2d b) +{ + return a.getX() * b.getX() + a.getY() * b.getY(); +} +void Vector2d::rotateBy(Vector2d &a, Rotation2d theta) +{ + Rotation2d temp(a.getOrientation().getRad() + theta.getRad()); + a.updateVectorfromPolar(a.getLength(), temp); +} \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/vector2d.h b/teensy40_brushed_v2/lib/Arm/vector2d.h new file mode 100644 index 0000000..f140d95 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/vector2d.h @@ -0,0 +1,117 @@ +#pragma once +#include "rotation2d.h" + +/** + * A simple class that represents a vector in 2d space. + * @author David LY + */ +class Vector2d +{ +private: + double _x, _y; + double _length; + Rotation2d _orientation; + +public: + /** + * Creates a new vector with given x and y components. + * @param x The x component of the vector. + * @param y The y component of the vector. + */ + + Vector2d(double x, double y); + /** + * Creates a new vector with given length and angle. + * @param length The length of the vector. + * @param orientation The angle of the vector. + */ + + Vector2d(double length, Rotation2d orientation); + /** + * Change vector with given x and y components. + * @param x The x component of the vector. + * @param y The y component of the vector. + */ + + void updateVectorfromCartesian(double x, double y); + /** + * Change vector with given length and angle. + * @param length The length of the vector. + * @param orientation The angle of the vector. + */ + void updateVectorfromPolar(double l, Rotation2d orientation); + + /** + * @return The x component of the vector. + */ + double getX() const; + + /** + * @return The y component of the vector. + */ + double getY() const; + + /** + * @return The length of the vector. + */ + double getLength() const; + + /** + * @return The orientaion of the vector. + */ + Rotation2d getOrientation() const; + + /** + * Scalar multiplication of the vector. + * @param scalar The scalar to multiply the vector by. + */ + void operator*=(double scalar); + + /** + * Vector addition of the vector. + * @param b The vector to add to the vector. + */ + void operator+=(Vector2d b); + + /** + * Vector subtraction of the vector. + * @param b The vector to subtract from the vector. + */ + + void operator-=(Vector2d b); + /** + * Scalar multiplication of the vector. + * New dynamic Vector2d object is created. + * @return new Vector2d multiplied by the scalar. + */ + Vector2d *operator*(double scalar) const; + + /** + * Vector addition of the vector. + * New dynamic Vector2d object is created. + * @return new Vector2d added to the vector. + */ + Vector2d *operator+(Vector2d b) const; + + /** + * Vector subtraction of the vector. + * New dynamic Vector2d object is created. + * @return new Vector2d subtracted from the vector. + */ + Vector2d *operator-(Vector2d b) const; + + /** + * Dot Product computation of two vector. + * @param a The first vector. + * @param b The second vector. + * @return The dot product of two vector. + */ + static double dot(Vector2d a, Vector2d b); + + /** + * Rotation of a vector + * @param a The vector to rotate. This vector is changed by the method. + * @param theta The angle to rotate the vector by. + */ + static void rotateBy(Vector2d &a, Rotation2d theta); +}; \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/velocity_estimation.cpp b/teensy40_brushed_v2/lib/Arm/velocity_estimation.cpp new file mode 100644 index 0000000..93bdf7a --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/velocity_estimation.cpp @@ -0,0 +1,150 @@ +#include "velocity_estimation.h" + +void velocity_estimation::initialize_parameters(float ppr, float error, float timeScale, uint8_t skip, uint32_t delay) +{ + _ppr = ppr; + _errorMargin = error; + _timeScale = timeScale; + _SKIP = skip; + _maxDelay = delay; + + _n = 0; + _size = 2; + _firstTime = 0; + _firstPos = 0; + + _foaw.newPos = 0; + _foaw.newtime = 0; + _foaw.lastPos = 0; + _foaw.lasttime = 0; + _foaw.k = 0; + + _mult = 360.0f * 1000000.0f / _ppr; + + for (uint8_t i = 0; i < 2; i++) + { + _coefficients[i] = 0.0; + } + + for (uint8_t i = 0; i < 5; i++) + { + _pastValues[i] = 0.0; + _sortedIndex[i] = i; + } + + for (uint8_t i = 0; i < FILTER_SIZE; i++) + { + _positions[i] = 0; + _timeStamps[i] = 0; + } + + for (uint8_t i = 0; i < 32; i++) + { + _foaw._positions[i] = 0; + _foaw._timeStamps[i] = 0; + } +} + +void velocity_estimation::update_readings(int32_t position, uint32_t timeStamp) +{ + _newEvent = 1; + + uint8_t count = _n % FILTER_SIZE; + + _foaw.newPos = position; + _foaw.newtime = timeStamp; + + _n++; +} + +int32_t velocity_estimation::get_last_position(void) +{ + return _positions[_n]; +} + +float velocity_estimation::foaw(int best) +{ + // As implemented by Steve Sinclair (https://github.com/radarsat1/FOAW/blob/master/foaw.c) + // adapted to our data by Antoine Weill--Duflos + int i, j, l; + uint32_t bad; + float b, p_mean, p_out, t_mean, t_out, dt; + float velocity = 0.0; + const float pos_err_max = 1.0; + const int time_err_max = 5; + const uint32_t max_outlier = 8; + + /* circular buffer */ + _foaw.k = (_foaw.k + 1) % size; + _foaw._positions[_foaw.k] = _foaw.newPos; + _foaw._timeStamps[_foaw.k] = _foaw.newtime; + + for (i = 1; i < size; i++) // size is n + { + // basically doing end fit instead of best fit -> why? + dt = (float)(_foaw._timeStamps[_foaw.k] - _foaw._timeStamps[(_foaw.k - i + size) % size]); + if (dt != 0) + { + + if (best) + { + b = 0.0f; + for (l = 0; l < (i + 1); l++) + { + b += (i - 2 * l) * _foaw._positions[(_foaw.k - l + size) % size]; + } + b = 6.0f * (float)b / ((dt * (i + 1) * (i + 2))); + } + else + { + b = (float)(_foaw._positions[_foaw.k] - _foaw._positions[(_foaw.k - i + size) % size]) / dt; + } + } + else + { + b = 0.0; + continue; + } + + bad = 0; + for (j = 1; j < i; j++) + { + p_out = _foaw._positions[(_foaw.k - j + size) % size]; + t_out = _foaw._timeStamps[(_foaw.k - j + size) % size]; + p_mean = _foaw._positions[_foaw.k] + b * (t_out - _foaw._timeStamps[_foaw.k]); + t_mean = (p_out - _foaw._positions[_foaw.k]) / b + _foaw._timeStamps[_foaw.k]; + + if (p_mean + pos_err_max < p_out || p_mean - pos_err_max > p_out || t_mean + time_err_max < t_out || t_mean - time_err_max > t_out) + { + bad++; + if (bad >= max_outlier) + break; + } + } + if (bad >= max_outlier) + break; + + velocity = _mult * b; // deg/s formulation + // velocity = b / _ppr * 2 * _pi * 1000000.0; // rad/s formulation + } + return velocity; +} + +void velocity_estimation::_compare_swap(int a, int b) +{ + int indexA = _sortedIndex[a]; + int indexB = _sortedIndex[b]; + + if (indexA > 4 || indexB > 4) + { + indexA = 0; + indexB = 1; + } + + if (_pastValues[indexA] < _pastValues[indexB]) + { + int temp = _sortedIndex[a]; + _sortedIndex[a] = _sortedIndex[b]; + _sortedIndex[b] = temp; + } +} \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/Arm/velocity_estimation.h b/teensy40_brushed_v2/lib/Arm/velocity_estimation.h new file mode 100644 index 0000000..2d16ca1 --- /dev/null +++ b/teensy40_brushed_v2/lib/Arm/velocity_estimation.h @@ -0,0 +1,62 @@ +#include +#include "Arduino.h" + +class velocity_estimation +{ +public: + void initialize_parameters(float ppr, float error, float timeScale, uint8_t skip, uint32_t delay); + + void update_readings(int32_t position, uint32_t timeStamp); + + int32_t get_last_position(void); // may be depreciated + + // float htt(uint32_t currentTime); + + typedef struct + { + int32_t _positions[64]; + uint32_t _timeStamps[64]; + float _vel[64]; + float _c; + int k; + int32_t lastPos; + uint32_t lasttime; + int32_t newPos; + uint32_t newtime; + } foaw_vars; + + float foaw(int best); + +private: + const uint8_t FILTER_SIZE = 16; + + const float _pi = 3.141592654; + + float _ppr; + float _errorMargin; + float _timeScale; + uint8_t _SKIP; + uint32_t _maxDelay; + float _mult = 0.0f; + uint32_t size = 32; + + float _coefficients[2]; + uint8_t _size; + uint8_t _n; + float _previousEvent; + uint8_t _newEvent; // may not be necessary + uint32_t _firstTime; + int32_t _firstPos; + float _pastValues[5]; + uint16_t _index; + uint32_t _processTime; + + int _sortedIndex[5]; + + foaw_vars _foaw; + + int32_t _positions[16]; + uint32_t _timeStamps[16]; + + void _compare_swap(int a, int b); +}; \ No newline at end of file diff --git a/teensy40_brushed_v2/lib/README b/teensy40_brushed_v2/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/teensy40_brushed_v2/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/teensy40_brushed_v2/platformio.ini b/teensy40_brushed_v2/platformio.ini new file mode 100644 index 0000000..a0eaa17 --- /dev/null +++ b/teensy40_brushed_v2/platformio.ini @@ -0,0 +1,19 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:teensy40] +platform = teensy +board = teensy40 +framework = arduino +monitor_speed = 115200 +lib_deps = + jchristensen/movingAvg@^2.3.1 + frankjoshua/Rosserial Arduino Library@^0.9.1 + diff --git a/teensy40_brushed_v2/src/common.h b/teensy40_brushed_v2/src/common.h new file mode 100644 index 0000000..02c1782 --- /dev/null +++ b/teensy40_brushed_v2/src/common.h @@ -0,0 +1 @@ +#define USE_ROS_FIRMWARE 1 \ No newline at end of file diff --git a/teensy40_brushed_v2/src/main.cpp b/teensy40_brushed_v2/src/main.cpp new file mode 100644 index 0000000..d968a53 --- /dev/null +++ b/teensy40_brushed_v2/src/main.cpp @@ -0,0 +1,509 @@ +#include "common.h" +#if USE_ROS_FIRMWARE == 0 +#include +#include "hardware_pins.h" +#include "driver_motor.h" +#include "model_encoder.h" +#include "model_sensor.h" +#include "motor.h" + +#include + +// CONFIGURATION +#define CONFIG_NON_ROS 1 +#define CONFIG_DIR_TESTER 2 +#define CONFIG_SINGLE_MOTOR_TESTER 3 +#define BRUSHED_BOARD_CONFIG CONFIG_SINGLE_MOTOR_TESTER +#define PID_PERIOD_MS 100 + +#define OUTA_Pin ENCPIN1_1 +#define OUTB_Pin ENCPIN1_2 + +// DUMB POINTERS +driver_motor mot1; +driver_motor mot2; +driver_motor mot3; + +// TESTER VARIABLES +int stage = 0; +int pos = 90; +int tolerance = 0.5; +volatile boolean is_homed = false; + +// FUNCTION DECLARATIONS +void process_serial_cmd(); +void print_encoder_info(); +void brushed_board_homing(); +void brushed_board_loop(); +void brushed_board_dir_tester(); +void brushed_board_singe_setpoint_loop(); + +void lim1ISR(); +void lim2ISR(); +void lim3ISR(); +void lim4ISR(); +void lim5ISR(); +void lim6ISR(); + +// Define the size of the moving average window +const uint32_t MOVING_AVERAGE_SIZE = 10; +float cur1_voltage_buffer[MOVING_AVERAGE_SIZE] = {0}; +float cur2_voltage_buffer[MOVING_AVERAGE_SIZE] = {0}; +float cur3_voltage_buffer[MOVING_AVERAGE_SIZE] = {0}; +int cur1_voltage_buffer_idx = 0; +int cur2_voltage_buffer_idx = 0; +int cur3_voltage_buffer_idx = 0; + +float moving_average(float new_reading, float *buffer, int buffer_size, int *buffer_idx) +{ + float sum = 0; + float oldest_reading = buffer[*buffer_idx]; + + // Add the new reading to the buffer. + buffer[*buffer_idx] = new_reading; + *buffer_idx = (*buffer_idx + 1) % buffer_size; // Circular buffer + + // Sum all readings. + for (int i = 0; i < buffer_size; i++) + { + sum += buffer[i]; + } + sum -= oldest_reading; + + return sum / buffer_size; +} + +// Ensure counter is initialized +int counter = 0; + +Rotation2d *current_rotation; + +// LIMIT SWITCHES +IntervalTimer lim_watchdog_timer; +const unsigned long DEBOUNCE_DELAY_MS = 50; + +volatile bool lim1_state = false; +volatile bool lim2_state = false; +volatile bool lim3_state = false; +volatile bool lim4_state = false; +volatile bool lim5_state = false; +volatile bool lim6_state = false; + +void setup() +{ + // Initialize Serial + SerialUSB.begin(115200); + SerialUSB.println("CONFIGURING BRUSHED BOARD"); + + std::unique_ptr enc1 = std::make_unique(); + std::unique_ptr enc2 = std::make_unique(); + std::unique_ptr enc3 = std::make_unique(); + + std::unique_ptr cur1 = std::make_unique(); + std::unique_ptr cur2 = std::make_unique(); + std::unique_ptr cur3 = std::make_unique(); + + // Initialize encoders + // 43000 clicks for wrist pitch, 32580 for wrist roll + // Only using 1 & 2 becase 1 & 3 conflicts + // Could be 32768 since it's a power of 2 + enc1->initialize_encoder(0, 0, 32580, 1); // new small servo estimate for resolution + enc2->initialize_encoder(0, 0, 43000, 2); + // enc3->initialize_encoder(0, 0, 43000, 3); + + // Initialize current sensors + cur1->initialize_sensor(CURRENT_SENSE_A); + cur3->initialize_sensor(CURRENT_SENSE_C); + cur2->initialize_sensor(CURRENT_SENSE_B); + + // Initialize motors + mot1.attach_encoder(std::move(enc1)); + mot2.attach_encoder(std::move(enc2)); + // mot3.attach_encoder(std::move(enc3)); + + mot1.attach_current_sensor(std::move(cur1)); + mot2.attach_current_sensor(std::move(cur2)); + mot3.attach_current_sensor(std::move(cur3)); + + // Motor init, forward logic is 1 + mot1.initialize_motor(1, PWMPIN1, DIRPIN1, nSLEEP1, 5.0, 0.0); + mot2.initialize_motor(1, PWMPIN2, DIRPIN2, nSLEEP2, 5.0, 0.0); + mot3.initialize_motor(1, PWMPIN3, DIRPIN3, nSLEEP3, 5.0, 0.0); + + // Set motor configuration after initialization + mot1.set_angle_limit_ps(wrist_pitch_max_angle, wrist_pitch_min_angle); + mot1.set_gear_ratio(2.0); + mot1._is_circular_joint = false; + + // Limit Switches + pinMode(LIM_1, INPUT); + pinMode(LIM_2, INPUT); + pinMode(LIM_3, INPUT); + pinMode(LIM_4, INPUT); + pinMode(LIM_5, INPUT); + pinMode(LIM_6, INPUT); + attachInterrupt(digitalPinToInterrupt(LIM_1), lim1ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_2), lim2ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_3), lim3ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_4), lim4ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_5), lim5ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_6), lim6ISR, CHANGE); + + // Watchdog timer to prevent bouncing or stuck limit switches + lim_watchdog_timer.begin([]() + { + lim1_state = digitalRead(LIM_1); + lim2_state = digitalRead(LIM_2); + lim3_state = digitalRead(LIM_3); + lim4_state = digitalRead(LIM_4); + lim5_state = digitalRead(LIM_5); + lim6_state = digitalRead(LIM_6); }, + DEBOUNCE_DELAY_MS * 1000); + + // Initialize Motor + // nSLEEP is actually hardwired to 5V so not really necessary + digitalWrite(nSLEEP1, HIGH); + digitalWrite(nSLEEP2, HIGH); + digitalWrite(nSLEEP3, HIGH); + + // Modelling + current_rotation = Rotation2d::getRotationFromDeg(0); + SerialUSB.println("DONE CONFIGURING BRUSHED BOARD"); + + // HOMING, only linear joints will be homed + SerialUSB.println("HOMING"); + brushed_board_homing(); + while (!is_homed) + ; + SerialUSB.println("DONE HOMING"); +} + +void loop() +{ +#if BRUSHED_BOARD_CONFIG == CONFIG_NON_ROS + brushed_board_loop(); +#elif BRUSHED_BOARD_CONFIG == CONFIG_DIR_TESTER + brushed_board_dir_tester(); +#elif BRUSHED_BOARD_CONFIG == CONFIG_SINGLE_MOTOR_TESTER + process_serial_cmd(); + brushed_board_singe_setpoint_loop(); +#endif +} + +void process_serial_cmd() +{ + static String inputString = ""; // A String to hold incoming data + static boolean inputStringComplete = false; // Whether the string is complete + + while (SerialUSB.available()) + { + char inChar = (char)SerialUSB.read(); // Read each character + if (inChar == '\n') + { + inputStringComplete = true; // If newline, input is complete + } + else + { + inputString += inChar; // Add character to input + } + } + + if (inputStringComplete) + { + SerialUSB.print("Received: "); + SerialUSB.println(inputString); // Echo the input for debugging + + // Process the completed command + if (inputString.startsWith("i ")) + { + // Increment command + float incrementValue = inputString.substring(2).toFloat(); // Extract number + float pos = mot1.get_target_angle_ps(); // Get current position + mot1.set_target_angle_ps(pos + incrementValue); // Increment and set new position + SerialUSB.print("Incremented position by: "); + SerialUSB.println(incrementValue); + } + else if (inputString.startsWith("s ")) + { + // Set command + pos = inputString.substring(2).toFloat(); // Extract and set new position + mot1.set_target_angle_ps(pos); + SerialUSB.print("Set position to: "); + SerialUSB.println(pos); + } + else + { + SerialUSB.println("Unknown command"); + } + + // Clear the string for the next command + inputString = ""; + inputStringComplete = false; + } +} + +void brushed_board_loop() +{ + delay(10); + + if (stage) + { + SerialUSB.println("Already reached target position"); + } + else + { + mot1.set_target_angle_ps(pos); + mot1.closed_loop_control_tick(); + float enc1_angle_ps = mot1.get_current_angle_ps(); + SerialUSB.printf("enc1_angle_ps: %8.4f \n", enc1_angle_ps); + + if (enc1_angle_ps <= pos + tolerance && enc1_angle_ps >= pos - tolerance && stage == 0) + { + SerialUSB.println("Reached target position"); + // pos = -60; + // stage++; + } + } +} + +// WRIST PITCH MAX LIMIT SWITCH +void lim1ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + is_homed = true; + mot1._encoder->set_current_angle_es(wrist_pitch_max_angle * mot1._gear_ratio); + SerialUSB.println("LIM_1 triggered"); + last_interrupt_time = interrupt_time; + lim1_state = digitalRead(LIM_1); + } +} + +// WRIST PITCH MIN LIMIT SWITCH +void lim2ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + mot1._encoder->set_current_angle_es(wrist_pitch_min_angle * mot1._gear_ratio); + SerialUSB.println("LIM_2 triggered"); + last_interrupt_time = interrupt_time; + lim2_state = digitalRead(LIM_2); + } +} + +void lim3ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + SerialUSB.println("LIM_3 triggered"); + last_interrupt_time = interrupt_time; + lim3_state = digitalRead(LIM_3); + } +} + +void lim4ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + SerialUSB.println("LIM_4 triggered"); + last_interrupt_time = interrupt_time; + lim4_state = digitalRead(LIM_4); + } +} + +void lim5ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + SerialUSB.println("LIM_5 triggered"); + last_interrupt_time = interrupt_time; + lim5_state = digitalRead(LIM_5); + } +} + +void lim6ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + SerialUSB.println("LIM_6 triggered"); + last_interrupt_time = interrupt_time; + lim6_state = digitalRead(LIM_6); + } +} + +// HOMING SEQUENCE, go forward until limit switch is triggered +void brushed_board_homing() +{ + // Only do homing for linear joints + if (!mot1._is_circular_joint) + { + mot1.set_direction(mot1._forward_dir); + mot1._pwm_write_duty(40); + } +} + +// PRINT ENCODER INFO +void print_encoder_info() +{ + // TEST LIMIT SWITCHES -------------------------------------------------------------------- + // printf("LIM_1: %d, LIM_2: %d, LIM_3: %d, LIM_4: %d, LIM_5: %d, LIM_6: %d\n", + // lim1_state, lim2_state, lim3_state, lim4_state, lim5_state, lim6_state); + + // TEST CURRENT SENSOR -------------------------------------------------------------------- + mot1._current_sensor->read_sensor_value(); + mot2._current_sensor->read_sensor_value(); + mot3._current_sensor->read_sensor_value(); + + // TODO check implementation of get_current() + float cur1_current = mot1._current_sensor->get_current(); + float cur2_current = mot2._current_sensor->get_current(); + float cur3_current = mot3._current_sensor->get_current(); + + // Middle value is 1.5V + float cur1_voltage = mot1._current_sensor->get_raw_voltage() - 1.5; + float cur2_voltage = mot2._current_sensor->get_raw_voltage() - 1.5; + float cur3_voltage = mot3._current_sensor->get_raw_voltage() - 1.5; + float smoothed_cur1_voltage = moving_average(cur1_voltage, cur1_voltage_buffer, MOVING_AVERAGE_SIZE, &cur1_voltage_buffer_idx); + float smoothed_cur2_voltage = moving_average(cur2_voltage, cur2_voltage_buffer, MOVING_AVERAGE_SIZE, &cur2_voltage_buffer_idx); + float smoothed_cur3_voltage = moving_average(cur3_voltage, cur3_voltage_buffer, MOVING_AVERAGE_SIZE, &cur3_voltage_buffer_idx); + + // SerialUSB.printf("cur1_current: %8.4f, cur2_current: %8.4f, cur3_current: %8.4f ", + // cur1_current, cur2_current, cur3_current); + SerialUSB.printf("cur1_voltage: %8.4f, cur2_voltage: %8.4f, cur3_voltage: %8.4f, ", + smoothed_cur1_voltage, smoothed_cur2_voltage, smoothed_cur3_voltage); + + // TEST ENCODER ------------------------------------------------------------------------------ + + // Channel 3 is conflicted, unused. + mot1._encoder->poll_encoder_angle(); + mot2._encoder->poll_encoder_angle(); + // mot3._encoder->poll_encoder_angle(); + + // Read encoder values. + float enc1_quad_enc_pos = mot1._encoder->_encoder->read(); + float enc1_angle_single = mot1._encoder->get_angle_single(); + float enc1_angle_es = mot1.get_current_angle_es(); + float enc1_angle_ps = mot1.get_current_angle_ps(); + float enc1_setpoint = mot1.get_target_angle_ps(); + + // float enc2_quad_enc_pos = mot2._encoder->_encoder->read(); + // float enc2_angle_single = mot2._encoder->get_angle_single(); + // float enc2_angle_multi = mot2._encoder->get_angle_multi(); + // float enc3_quad_enc_pos = mot3._encoder->_encoder->read(); + // float enc3_angle_single = mot3._encoder->get_angle_single(); + // float enc3_angle_multi = mot3._encoder->get_angle_multi(); + + SerialUSB.printf("enc1_quad_enc_pos: %8.4f, enc1_angle_single: %8.4f, enc1_angle_es: %8.4f, enc1_angle_ps: %8.4f, enc1_setpoint: %8.4f, ", + enc1_quad_enc_pos, enc1_angle_single, enc1_angle_es, enc1_angle_ps, enc1_setpoint); + + // float enc1_rev = mot1._encoder->_encoder->getRevolution(); + // float enc1_hold_rev = mot1._encoder->_encoder->getHoldRevolution(); + // SerialUSB.printf("enc1_rev: %8.4f, enc1_hold_rev: %8.4f, ", enc1_rev, enc1_hold_rev); + if (enc1_angle_es <= 2881 && enc1_angle_es >= 2879) + { + SerialUSB.printf("\n2880 here^"); + analogWrite(PWMPIN1, 0); + while (true) + ; + } + SerialUSB.println(); +} + +void brushed_board_dir_tester() +{ + delay(PID_PERIOD_MS); + + // Run Motors + mot1._pwm_write_duty(180); + mot2._pwm_write_duty(60); + mot3._pwm_write_duty(60); + + // Print Encoder Info + print_encoder_info(); + + // TEST GRAVITY COMPENSATION -------------------------------------------------------------------- + // double output; + // current_rotation->setAngleRad(current_angle / 360 * 2 * PI); + // maintainStateProto(*current_rotation, &output); + // printf("current output: %8.4f\r\n", output); + // printf("Current voltage: %8.4f\n", output); + // if (output < 0) + // { + // digitalWrite(DIR1, HIGH); + // } + // else + // { + // digitalWrite(DIR1, LOW); + // } + // int analog_write_output = (output / 24) * 255; + // printf("Current analog output: %d\n", analog_write_output); + // analogWrite(PWM1, analog_write_output); + + // ENCODER PINS TEST -------------------------------------------------------------------- + // SerialUSB.printf("ENCPIN1_1: %d, ENCPIN1_2: %d ", digitalRead(ENCPIN1_1), digitalRead(ENCPIN1_2)); + // SerialUSB.printf("ENCPIN2_1: %d, ENCPIN2_2: %d ", digitalRead(ENCPIN2_1), digitalRead(ENCPIN2_2)); + // SerialUSB.printf("ENCPIN3_1: %d, ENCPIN3_2: %d\n", digitalRead(ENCPIN3_1), digitalRead(ENCPIN3_2)); + // if (digitalRead(OUTA_Pin) == LOW) // If OUTA is LOW + // { + // if (digitalRead(OUTB_Pin) == LOW) // If OUTB is also LOW... CCK + // { + // while (digitalRead(OUTB_Pin) == LOW) + // { + // }; // wait for OUTB to go HIGH + // counter--; + // while (digitalRead(OUTA_Pin) == LOW) + // { + // }; // wait for OUTA to go HIGH + // delay(10); // wait for some more time + // } + // else if (digitalRead(OUTB_Pin) == HIGH) // If OUTB is HIGH + // { + // while (digitalRead(OUTB_Pin) == HIGH) + // { + // }; // wait for OUTB to go LOW.. CK + // counter++; + // while (digitalRead(OUTA_Pin) == LOW) + // { + // }; // wait for OUTA to go HIGH + // while (digitalRead(OUTB_Pin) == LOW) + // { + // }; // wait for OUTB to go HIGH + // delay(10); // wait for some more time + // } + + // if (counter < 0) + // counter = 0; + // if (counter > 180) + // counter = 180; + // } + // SerialUSB.println(counter); +} + +void brushed_board_singe_setpoint_loop() +{ + delay(PID_PERIOD_MS); + + mot1.closed_loop_control_tick(); + + // Print Encoder Info + print_encoder_info(); +} +#endif // USE_ROS_FIRMWARE == 0 \ No newline at end of file diff --git a/teensy40_brushed_v2/src/main_ros.cpp b/teensy40_brushed_v2/src/main_ros.cpp new file mode 100644 index 0000000..4fd8959 --- /dev/null +++ b/teensy40_brushed_v2/src/main_ros.cpp @@ -0,0 +1,469 @@ +#include "common.h" +#if USE_ROS_FIRMWARE == 1 +#include +#include "hardware_pins.h" +#include "driver_motor.h" +#include "model_encoder.h" +#include "model_sensor.h" +#include "motor.h" + +#include +#include "std_msgs/Float32.h" +#include "std_msgs/Float32MultiArray.h" + +#include + +// CONFIGURATION +#define PID_PERIOD_MS 100 +#define PID_PERIOD_US PID_PERIOD_MS * 1000 +#define HWSERIAL Serial2 +#define DEBUG_PRINT 1 + +#define OUTA_Pin ENCPIN1_1 +#define OUTB_Pin ENCPIN1_2 + +// ROS +ros::NodeHandle nh; +float arm_brushed_setpoint_ps[3] = {0, 0, 0}; +float arm_brushed_angle_ps[3] = {0, 0, 0}; +void brushed_board_ros_loop(); +void arm_brushed_cmd_cb(const std_msgs::Float32MultiArray &input_msg); +std_msgs::Float32MultiArray arm_brushed_fb_msg; +ros::Publisher arm_brushed_fb_pub("/armBrushedFb", &arm_brushed_fb_msg); +ros::Subscriber arm_brushed_cmd_sub("/armBrushedCmd", arm_brushed_cmd_cb); + +// DUMB POINTERS +driver_motor mot1; +driver_motor mot2; +driver_motor mot3; + +// TESTER VARIABLES +int stage = 0; +int pos = 90; +int tolerance = 0.5; +volatile boolean is_homed = false; + +// FUNCTION DECLARATIONS +void brushed_board_homing(); +void brushed_board_ros_loop(); + +void lim1ISR(); +void lim2ISR(); +void lim3ISR(); +void lim4ISR(); +void lim5ISR(); +void lim6ISR(); + +// Define the size of the moving average window +const uint32_t MOVING_AVERAGE_SIZE = 10; +float cur1_voltage_buffer[MOVING_AVERAGE_SIZE] = {0}; +float cur2_voltage_buffer[MOVING_AVERAGE_SIZE] = {0}; +float cur3_voltage_buffer[MOVING_AVERAGE_SIZE] = {0}; +int cur1_voltage_buffer_idx = 0; +int cur2_voltage_buffer_idx = 0; +int cur3_voltage_buffer_idx = 0; + +float moving_average(float new_reading, float *buffer, int buffer_size, int *buffer_idx) +{ + float sum = 0; + float oldest_reading = buffer[*buffer_idx]; + + // Add the new reading to the buffer. + buffer[*buffer_idx] = new_reading; + *buffer_idx = (*buffer_idx + 1) % buffer_size; // Circular buffer + + // Sum all readings. + for (int i = 0; i < buffer_size; i++) + { + sum += buffer[i]; + } + sum -= oldest_reading; + + return sum / buffer_size; +} + +// Ensure counter is initialized +int counter = 0; + +Rotation2d *current_rotation; + +// LIMIT SWITCHES +IntervalTimer lim_watchdog_timer; +const unsigned long DEBOUNCE_DELAY_MS = 50; + +volatile bool lim1_state = false; +volatile bool lim2_state = false; +volatile bool lim3_state = false; +volatile bool lim4_state = false; +volatile bool lim5_state = false; +volatile bool lim6_state = false; + +uint32_t loop_last_time = 0; + +void setup() +{ + HWSERIAL.begin(115200); + while (!HWSERIAL) + ; + HWSERIAL.println("Serial port initialized"); + // Initialize ROS + nh.initNode(); + nh.advertise(arm_brushed_fb_pub); + nh.subscribe(arm_brushed_cmd_sub); + nh.negotiateTopics(); + while (!nh.connected()) + { + nh.negotiateTopics(); + // nh.spinOnce(); + } + + std::unique_ptr enc1 = std::make_unique(); + std::unique_ptr enc2 = std::make_unique(); + std::unique_ptr enc3 = std::make_unique(); + + std::unique_ptr cur1 = std::make_unique(); + std::unique_ptr cur2 = std::make_unique(); + std::unique_ptr cur3 = std::make_unique(); + + // Initialize encoders + // 43000 clicks for wrist pitch, 32580 for wrist roll + // Only using 1 & 2 becase 1 & 3 conflicts + // Could be 32768 since it's a power of 2 + // enc1->initialize_encoder(0, 0, 32580, 1); // new small servo estimate for resolution + enc1->initialize_encoder(0, 0, 43000, 1); + enc2->initialize_encoder(0, 0, 32580, 2); + // enc3->initialize_encoder(0, 0, 43000, 3); + + // Initialize current sensors + cur1->initialize_sensor(CURRENT_SENSE_A); + cur3->initialize_sensor(CURRENT_SENSE_C); + cur2->initialize_sensor(CURRENT_SENSE_B); + + // Initialize motors + mot1.attach_encoder(std::move(enc1)); + mot2.attach_encoder(std::move(enc2)); + // mot3.attach_encoder(std::move(enc3)); + + mot1.attach_current_sensor(std::move(cur1)); + mot2.attach_current_sensor(std::move(cur2)); + mot3.attach_current_sensor(std::move(cur3)); + + // Motor init, forward logic is 1 + mot1.initialize_motor(1, PWMPIN1, DIRPIN1, nSLEEP1, 5.0, 0.0); + mot2.initialize_motor(1, PWMPIN2, DIRPIN2, nSLEEP2, 5.0, 0.0); + mot3.initialize_motor(1, PWMPIN3, DIRPIN3, nSLEEP3, 5.0, 0.0); + + // Set motor configuration after initialization + // Gear ratio needs to be set before angle limits so limits are scaled + mot1.set_gear_ratio(2.0); + mot1.set_angle_limit_ps(wrist_pitch_max_angle, wrist_pitch_min_angle); + mot1._is_circular_joint = false; + + mot2.set_gear_ratio(2.0); + mot2._is_circular_joint = true; + + // Limit Switches + pinMode(LIM_1, INPUT); + pinMode(LIM_2, INPUT); + pinMode(LIM_3, INPUT); + pinMode(LIM_4, INPUT); + pinMode(LIM_5, INPUT); + pinMode(LIM_6, INPUT); + attachInterrupt(digitalPinToInterrupt(LIM_1), lim1ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_2), lim2ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_3), lim3ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_4), lim4ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_5), lim5ISR, CHANGE); + attachInterrupt(digitalPinToInterrupt(LIM_6), lim6ISR, CHANGE); + + // Watchdog timer to prevent bouncing or stuck limit switches + lim_watchdog_timer.begin([]() + { + lim1_state = digitalRead(LIM_1); + lim2_state = digitalRead(LIM_2); + lim3_state = digitalRead(LIM_3); + lim4_state = digitalRead(LIM_4); + lim5_state = digitalRead(LIM_5); + lim6_state = digitalRead(LIM_6); }, + DEBOUNCE_DELAY_MS * 1000); + + // Initialize Motor + // nSLEEP is actually hardwired to 5V so not really necessary + digitalWrite(nSLEEP1, HIGH); + digitalWrite(nSLEEP2, HIGH); + digitalWrite(nSLEEP3, HIGH); + + // Modelling + current_rotation = Rotation2d::getRotationFromDeg(0); + + // HOMING, only linear joints will be homed + // brushed_board_homing(); + // while (!is_homed) + // ; + + loop_last_time = micros(); +} + +void loop() +{ + brushed_board_ros_loop(); +} + +// WRIST PITCH MAX LIMIT SWITCH +void lim1ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + is_homed = true; + mot1._encoder->set_current_angle_es(wrist_pitch_max_angle * mot1._gear_ratio); + last_interrupt_time = interrupt_time; + lim1_state = digitalRead(LIM_1); + } +} + +// WRIST PITCH MIN LIMIT SWITCH +void lim2ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + mot1._encoder->set_current_angle_es(wrist_pitch_min_angle * mot1._gear_ratio); + last_interrupt_time = interrupt_time; + lim2_state = digitalRead(LIM_2); + } +} + +void lim3ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + last_interrupt_time = interrupt_time; + lim3_state = digitalRead(LIM_3); + } +} + +void lim4ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + last_interrupt_time = interrupt_time; + lim4_state = digitalRead(LIM_4); + } +} + +void lim5ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + last_interrupt_time = interrupt_time; + lim5_state = digitalRead(LIM_5); + } +} + +void lim6ISR() +{ + static unsigned long last_interrupt_time = 0; + unsigned long interrupt_time = millis(); + + if (interrupt_time - last_interrupt_time > DEBOUNCE_DELAY_MS) + { + last_interrupt_time = interrupt_time; + lim6_state = digitalRead(LIM_6); + } +} + +// HOMING SEQUENCE, go forward until limit switch is triggered +void brushed_board_homing() +{ + // Only do homing for linear joints + if (!mot1._is_circular_joint) + { + mot1.set_direction(mot1._forward_dir); + mot1._pwm_write_duty(40); + } +} + +void process_serial_cmd() +{ + static String inputString = ""; // A String to hold incoming data + static boolean inputStringComplete = false; // Whether the string is complete + + while (HWSERIAL.available()) + { + char inChar = (char)HWSERIAL.read(); // Read each character + if (inChar == '\n') + { + inputStringComplete = true; // If newline, input is complete + } + else + { + inputString += inChar; // Add character to input + } + } + + if (inputStringComplete) + { + HWSERIAL.print("Received: "); + HWSERIAL.println(inputString); // Echo the input for debugging + + // Process the completed command + if (inputString.startsWith("i ")) + { + // Increment command + float incrementValue = inputString.substring(2).toFloat(); // Extract number + float pos = mot1.get_target_angle_ps(); // Get current position + mot1.set_target_angle_ps(pos + incrementValue); // Increment and set new position + HWSERIAL.print("Incremented position by: "); + HWSERIAL.println(incrementValue); + } + else if (inputString.startsWith("s ")) + { + // Set command + pos = inputString.substring(2).toFloat(); // Extract and set new position + mot1.set_target_angle_ps(pos); + HWSERIAL.print("Set position to: "); + HWSERIAL.println(pos); + } + else + { + HWSERIAL.println("Unknown command"); + } + + // Clear the string for the next command + inputString = ""; + inputStringComplete = false; + } +} + +// PRINT ENCODER INFO +void print_encoder_info() +{ + // TEST LIMIT SWITCHES -------------------------------------------------------------------- + // printf("LIM_1: %d, LIM_2: %d, LIM_3: %d, LIM_4: %d, LIM_5: %d, LIM_6: %d\n", + // lim1_state, lim2_state, lim3_state, lim4_state, lim5_state, lim6_state); + + // TEST CURRENT SENSOR -------------------------------------------------------------------- + mot1._current_sensor->read_sensor_value(); + mot2._current_sensor->read_sensor_value(); + mot3._current_sensor->read_sensor_value(); + + // TODO check implementation of get_current() + float cur1_current = mot1._current_sensor->get_current(); + float cur2_current = mot2._current_sensor->get_current(); + float cur3_current = mot3._current_sensor->get_current(); + + // Middle value is 1.5V + float cur1_voltage = mot1._current_sensor->get_raw_voltage() - 1.5; + float cur2_voltage = mot2._current_sensor->get_raw_voltage() - 1.5; + float cur3_voltage = mot3._current_sensor->get_raw_voltage() - 1.5; + float smoothed_cur1_voltage = moving_average(cur1_voltage, cur1_voltage_buffer, MOVING_AVERAGE_SIZE, &cur1_voltage_buffer_idx); + float smoothed_cur2_voltage = moving_average(cur2_voltage, cur2_voltage_buffer, MOVING_AVERAGE_SIZE, &cur2_voltage_buffer_idx); + float smoothed_cur3_voltage = moving_average(cur3_voltage, cur3_voltage_buffer, MOVING_AVERAGE_SIZE, &cur3_voltage_buffer_idx); + + // HWSERIAL.printf("cur1_current: %8.4f, cur2_current: %8.4f, cur3_current: %8.4f ", + // cur1_current, cur2_current, cur3_current); + // HWSERIAL.printf("cur1_voltage: %8.4f, cur2_voltage: %8.4f, cur3_voltage: %8.4f, ", + // smoothed_cur1_voltage, smoothed_cur2_voltage, smoothed_cur3_voltage); + + // TEST ENCODER ------------------------------------------------------------------------------ + + // Channel 3 is conflicted, unused. + mot1._encoder->poll_encoder_angle(); + mot2._encoder->poll_encoder_angle(); + // mot3._encoder->poll_encoder_angle(); + + // Read encoder values. + float enc1_quad_enc_pos = mot1._encoder->_encoder->read(); + float enc1_angle_single = mot1._encoder->get_angle_single(); + float enc1_angle_es = mot1.get_current_angle_es(); + float enc1_angle_ps = mot1.get_current_angle_ps(); + float enc1_setpoint = mot1.get_target_angle_ps(); + + float enc2_quad_enc_pos = mot2._encoder->_encoder->read(); + float enc2_angle_single = mot2._encoder->get_angle_single(); + float enc2_angle_es = mot2.get_current_angle_es(); + float enc2_angle_ps = mot2.get_current_angle_ps(); + float enc2_setpoint = mot2.get_target_angle_ps(); + + // float enc2_quad_enc_pos = mot2._encoder->_encoder->read(); + // float enc2_angle_single = mot2._encoder->get_angle_single(); + // float enc2_angle_multi = mot2._encoder->get_angle_multi(); + // float enc3_quad_enc_pos = mot3._encoder->_encoder->read(); + // float enc3_angle_single = mot3._encoder->get_angle_single(); + // float enc3_angle_multi = mot3._encoder->get_angle_multi(); + + HWSERIAL.printf("enc1_quad_enc_pos: %8.4f, enc1_angle_single: %8.4f, enc1_angle_es: %8.4f, enc1_angle_ps: %8.4f, enc1_setpoint: %8.4f, ", + enc1_quad_enc_pos, enc1_angle_single, enc1_angle_es, enc1_angle_ps, enc1_setpoint); + HWSERIAL.println(); + HWSERIAL.printf("enc2_quad_enc_pos: %8.4f, enc2_angle_single: %8.4f, enc2_angle_es: %8.4f, enc2_angle_ps: %8.4f, enc2_setpoint: %8.4f, ", + enc2_quad_enc_pos, enc2_angle_single, enc2_angle_es, enc2_angle_ps, enc2_setpoint); + + // float enc1_rev = mot1._encoder->_encoder->getRevolution(); + // float enc1_hold_rev = mot1._encoder->_encoder->getHoldRevolution(); + // HWSERIAL.printf("enc1_rev: %8.4f, enc1_hold_rev: %8.4f, ", enc1_rev, enc1_hold_rev); + HWSERIAL.println(); +} + +void brushed_board_ros_loop() +{ +#if DEBUG_PRINT == 1 + process_serial_cmd(); +#endif + // delay(PID_PERIOD_MS); + + uint32_t current_time = micros(); + if (current_time - loop_last_time > PID_PERIOD_US) + { + counter = current_time; + mot1.closed_loop_control_tick(); + mot2.closed_loop_control_tick(); + // mot3.closed_loop_control_tick(); + loop_last_time = current_time; +#if DEBUG_PRINT == 1 + print_encoder_info(); +#endif + } + + // Feedback + arm_brushed_angle_ps[0] = mot1.get_current_angle_ps(); + arm_brushed_angle_ps[1] = mot2.get_current_angle_ps(); + // arm_brushed_angle_ps[2] = mot3.get_current_angle_ps(); + arm_brushed_angle_ps[2] = 0.0; + + arm_brushed_fb_msg.data_length = 3; + arm_brushed_fb_msg.data = arm_brushed_angle_ps; + arm_brushed_fb_pub.publish(&arm_brushed_fb_msg); + + nh.spinOnce(); + delay(1); +} + +void arm_brushed_cmd_cb(const std_msgs::Float32MultiArray &input_msg) +{ + // 0 is WP, 1 is WR, 2 is EE + arm_brushed_setpoint_ps[0] = input_msg.data[2]; + arm_brushed_setpoint_ps[1] = input_msg.data[1]; + arm_brushed_setpoint_ps[2] = input_msg.data[0]; + mot1.set_target_angle_ps(arm_brushed_setpoint_ps[0]); + mot2.set_target_angle_ps(arm_brushed_setpoint_ps[1]); + HWSERIAL.printf("WP: %8.4f, WR: %8.4f, EE: %8.4f\n", arm_brushed_setpoint_ps[0], arm_brushed_setpoint_ps[1], arm_brushed_setpoint_ps[2]); + + // Motor 3 is controlled like a forklift, only up and down, range -1 to 1 + // mot3.set_target_angle_ps(arm_brushed_setpoint_ps[2]); + mot3.move_manual(arm_brushed_setpoint_ps[2]); +} +#endif // USE_ROS_FIRMWARE == 1 \ No newline at end of file diff --git a/teensy40_brushed_v2/test/README b/teensy40_brushed_v2/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/teensy40_brushed_v2/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html