124 lines
4.1 KiB
C++
124 lines
4.1 KiB
C++
/*
|
|
* servo-control.hpp
|
|
*
|
|
* Created on: 12.01.2021
|
|
* Author: frank
|
|
*/
|
|
|
|
#include <ax12.h>
|
|
#include "noise.hpp"
|
|
#include "SimplexNoise.h" //Library from https://github.com/jshaw/SimplexNoise
|
|
|
|
|
|
|
|
#ifndef SERVO_CONTROL_HPP_
|
|
#define SERVO_CONTROL_HPP_
|
|
|
|
#define SERVO_COUNT 5
|
|
|
|
struct Mood{
|
|
float shakiness;
|
|
float wakefulness;
|
|
float loneliness;
|
|
};
|
|
extern Mood mood;
|
|
|
|
#define RPM_TO_SPEED_RATIO 0.111
|
|
#define ANGLE_PER_DIGIT_RATIO .2925
|
|
//#define ANGLE_PER_DIGIT_RATIO 0.29 // From datasheet
|
|
|
|
#define MOTOR_TORQUE_LIMIT 100
|
|
#define MOTOR_TORQUE_RATIO 0.1
|
|
#define MOTOR_TORQUE_DIRECTION_MASK 0x00000200
|
|
#define MOTOR_TORQUE_VALUE_MASK 0x000001ff
|
|
|
|
#define BASE_MOTOR_INITIAL_POSITION_DEG 150
|
|
#define BASE_MOTOR_INITIAL_SPEED_RPM 5
|
|
#define BASE_MOTOR_MAX_ANGLE_DEG 48
|
|
#define BASE_MOTOR_TORQUE_LIMIT MOTOR_TORQUE_LIMIT
|
|
|
|
#define MID_MOTOR_INITIAL_POSITION_DEG 150
|
|
#define MID_MOTOR_INITIAL_SPEED_RPM 7
|
|
#define MID_MOTOR_MAX_ANGLE_DEG 105
|
|
#define MID_MOTOR_TORQUE_LIMIT MOTOR_TORQUE_LIMIT
|
|
|
|
#define NOSE_MOTOR_INITIAL_POSITION_DEG 150
|
|
#define NOSE_MOTOR_INITIAL_SPEED_RPM 10
|
|
#define NOSE_MOTOR_MAX_ANGLE_DEG 105
|
|
#define NOSE_MOTOR_TORQUE_LIMIT MOTOR_TORQUE_LIMIT
|
|
|
|
#define DXL_BASE_MOTOR_INITIAL_POSITION BASE_MOTOR_INITIAL_POSITION_DEG / ANGLE_PER_DIGIT_RATIO
|
|
#define DXL_BASE_MOTOR_INITIAL_SPEED BASE_MOTOR_INITIAL_SPEED_RPM / RPM_TO_SPEED_RATIO
|
|
#define DXL_BASE_MOTOR_MAX_ANGLE BASE_MOTOR_MAX_ANGLE_DEG / ANGLE_PER_DIGIT_RATIO
|
|
#define DXL_BASE_MOTOR_MIN_POSITION_CW DXL_BASE_MOTOR_INITIAL_POSITION - DXL_BASE_MOTOR_MAX_ANGLE
|
|
#define DXL_BASE_MOTOR_MAX_POSITION_CCW DXL_BASE_MOTOR_INITIAL_POSITION + DXL_BASE_MOTOR_MAX_ANGLE
|
|
#define DXL_BASE_MOTOR_TORQUE_LIMIT BASE_MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO
|
|
|
|
#define DXL_MID_MOTOR_INITIAL_POSITION MID_MOTOR_INITIAL_POSITION_DEG / ANGLE_PER_DIGIT_RATIO
|
|
#define DXL_MID_MOTOR_INITIAL_SPEED MID_MOTOR_INITIAL_SPEED_RPM / RPM_TO_SPEED_RATIO
|
|
#define DXL_MID_MOTOR_MAX_ANGLE MID_MOTOR_MAX_ANGLE_DEG / ANGLE_PER_DIGIT_RATIO
|
|
#define DXL_MID_MOTOR_MIN_POSITION_CW DXL_MID_MOTOR_INITIAL_POSITION - DXL_MID_MOTOR_MAX_ANGLE
|
|
#define DXL_MID_MOTOR_MAX_POSITION_CCW DXL_MID_MOTOR_INITIAL_POSITION + DXL_MID_MOTOR_MAX_ANGLE
|
|
#define DXL_MID_MOTOR_TORQUE_LIMIT MID_MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO
|
|
|
|
#define DXL_NOSE_MOTOR_INITIAL_POSITION NOSE_MOTOR_INITIAL_POSITION_DEG / ANGLE_PER_DIGIT_RATIO
|
|
#define DXL_NOSE_MOTOR_INITIAL_SPEED NOSE_MOTOR_INITIAL_SPEED_RPM / RPM_TO_SPEED_RATIO
|
|
#define DXL_NOSE_MOTOR_MAX_ANGLE NOSE_MOTOR_MAX_ANGLE_DEG / ANGLE_PER_DIGIT_RATIO
|
|
#define DXL_NOSE_MOTOR_MIN_POSITION_CW DXL_NOSE_MOTOR_INITIAL_POSITION - DXL_NOSE_MOTOR_MAX_ANGLE
|
|
#define DXL_NOSE_MOTOR_MAX_POSITION_CCW DXL_NOSE_MOTOR_INITIAL_POSITION + DXL_NOSE_MOTOR_MAX_ANGLE
|
|
#define DXL_NOSE_MOTOR_TORQUE_LIMIT NOSE_MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO
|
|
|
|
struct dxl_servo {
|
|
int id;
|
|
int initial_speed;
|
|
int initial_position;
|
|
int min_position;
|
|
int max_position;
|
|
int torque_limit;
|
|
int current_torque;
|
|
int joint_orientation;
|
|
};
|
|
|
|
void matrix_weights_update();
|
|
|
|
void servos_setup();
|
|
void servos_set_current_initial_position();
|
|
|
|
void servos_init_with_fixed_position();
|
|
void servos_init_with_current_position();
|
|
|
|
void servo_init(const int servo_id);
|
|
void servos_init();
|
|
|
|
void servo_to_initial_position(const int servo_id);
|
|
void servos_to_initial_position();
|
|
|
|
int servo_moving(const int servo_id);
|
|
int servos_moving();
|
|
|
|
void servos_in_position_wait();
|
|
void servo_in_position_wait(int servo_id);
|
|
|
|
int servo_set_speed_rpm(const int servo_id, const int rpm);
|
|
|
|
void servo_move_to_angle(const int servo_id, const int deg);
|
|
void servos_move_to_angle(const int deg);
|
|
|
|
void servos_collect_current_torque();
|
|
void servos_print_current_torque();
|
|
|
|
void map_roll_to_servos_and_move(const int roll);
|
|
void map_pitch_to_servos_and_move(const int pitch);
|
|
|
|
void map_angle_to_servo_with_initial_position(const int servo_id, const int deg);
|
|
void map_roll_to_servos_with_initial_position_and_move(const int roll);
|
|
void map_pitch_to_servos_with_initial_position_and_move(const int pitch);
|
|
|
|
void map_servos_by_weights_with_initial_position_and_move(const int roll, const int pitch);
|
|
|
|
void read_servos_positions();
|
|
|
|
void transmit_matrix_weights();
|
|
|
|
|
|
#endif /* SERVO_CONTROL_HPP_ */
|