275 lines
8.5 KiB
C++
275 lines
8.5 KiB
C++
/*
|
|
* servo-control.cpp
|
|
*
|
|
* Created on: 12.01.2021
|
|
* Author: frank
|
|
*/
|
|
|
|
#include "servo-control.hpp"
|
|
|
|
int angle_to_dxl_servo_position(const int deg);
|
|
int rpm_to_dxl_servo_speed(const int rpm);
|
|
|
|
|
|
Mood mood;
|
|
|
|
float matrix_weights[SERVO_COUNT][3]; //mapping outputs, sensors/moods
|
|
#define W_PITCH 0
|
|
#define W_ROLL 1
|
|
#define W_NOISE 2
|
|
|
|
|
|
void matrix_weights_update(){
|
|
//0=pitch
|
|
//1=roll
|
|
|
|
|
|
float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
|
|
|
|
|
|
|
|
|
|
//testing on servo
|
|
matrix_weights[0][W_NOISE]=90.0*map_mode;
|
|
matrix_weights[0][W_ROLL]=1.0* (1-map_mode);
|
|
|
|
matrix_weights[1][W_NOISE]=90.0*map_mode;
|
|
matrix_weights[1][W_PITCH]=1.0* (1-map_mode);
|
|
|
|
matrix_weights[2][W_NOISE]=90.0*map_mode;
|
|
matrix_weights[2][W_ROLL]=1.0* (1-map_mode);
|
|
|
|
matrix_weights[3][W_NOISE]=90.0*map_mode;
|
|
matrix_weights[3][W_PITCH]=1.0* (1-map_mode);
|
|
|
|
matrix_weights[4][W_NOISE]=90.0*map_mode;
|
|
matrix_weights[4][W_ROLL]=1.0* (1-map_mode);
|
|
|
|
|
|
|
|
float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.02, 0,1),0,1);
|
|
for(int i = 0; i < SERVO_COUNT ; i++){
|
|
matrix_weights[i][W_PITCH]*=sleeping;
|
|
matrix_weights[i][W_ROLL]*=sleeping;
|
|
matrix_weights[i][W_NOISE]*=sleeping;
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
dxl_servo servos[SERVO_COUNT];
|
|
|
|
void servos_set_current_initial_position() {
|
|
for(int i = 0; i < SERVO_COUNT ; i++){
|
|
servos[i].initial_position = dxlGetPosition(servos[i].id);
|
|
}
|
|
}
|
|
|
|
void servos_setup() {
|
|
servos[0].id = 1;
|
|
servos[0].initial_position = DXL_NOSE_MOTOR_INITIAL_POSITION;
|
|
servos[0].initial_speed = DXL_NOSE_MOTOR_INITIAL_SPEED;
|
|
servos[0].max_position = DXL_NOSE_MOTOR_MAX_POSITION_CCW;
|
|
servos[0].min_position = DXL_NOSE_MOTOR_MIN_POSITION_CW;
|
|
servos[0].torque_limit = DXL_NOSE_MOTOR_TORQUE_LIMIT;
|
|
servos[0].joint_orientation = - 1;
|
|
|
|
servos[1].id = 2;
|
|
servos[1].initial_position = DXL_MID_MOTOR_INITIAL_POSITION;
|
|
servos[1].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
|
|
servos[1].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
|
|
servos[1].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
|
|
servos[1].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT;
|
|
servos[1].joint_orientation = - 1;
|
|
|
|
servos[2].id = 3;
|
|
servos[2].initial_position = DXL_MID_MOTOR_INITIAL_POSITION;
|
|
servos[2].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
|
|
servos[2].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
|
|
servos[2].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
|
|
servos[2].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT;
|
|
servos[2].joint_orientation = + 1;
|
|
|
|
servos[3].id = 4;
|
|
servos[3].initial_position = DXL_MID_MOTOR_INITIAL_POSITION;
|
|
servos[3].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
|
|
servos[3].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
|
|
servos[3].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
|
|
servos[3].torque_limit = DXL_MID_MOTOR_TORQUE_LIMIT;
|
|
servos[3].joint_orientation = + 1;
|
|
|
|
servos[4].id = 5;
|
|
servos[4].initial_position = DXL_BASE_MOTOR_INITIAL_POSITION;
|
|
servos[4].initial_speed = DXL_BASE_MOTOR_INITIAL_SPEED;
|
|
servos[4].max_position = DXL_BASE_MOTOR_MAX_POSITION_CCW;
|
|
servos[4].min_position = DXL_BASE_MOTOR_MIN_POSITION_CW;
|
|
servos[4].torque_limit = DXL_BASE_MOTOR_TORQUE_LIMIT;
|
|
servos[4].joint_orientation = - 1;
|
|
}
|
|
|
|
void servos_init_with_fixed_position() {
|
|
servos_setup();
|
|
servos_init();
|
|
}
|
|
|
|
void servos_init_with_current_position() {
|
|
servos_setup();
|
|
servos_set_current_initial_position();
|
|
servos_init();
|
|
}
|
|
|
|
void servos_init() {
|
|
for(int i = 0; i< SERVO_COUNT; i++) {
|
|
dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board
|
|
dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position);
|
|
dxlSetGoalSpeed(servos[i].id, servos[i].initial_speed);
|
|
dxlTorqueOn(servos[i].id);
|
|
dxlSetRunningTorqueLimit(servos[i].id,MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO);
|
|
dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
|
|
}
|
|
}
|
|
|
|
void servo_init(const int servo_id) {
|
|
int index = servo_id - 1;
|
|
dxlSetJointMode(servos[index].id, servos[index].min_position, servos[index].max_position);
|
|
dxlSetGoalSpeed(servos[index].id, servos[index].initial_speed);
|
|
dxlSetGoalPosition(servos[index].id, servos[index].initial_position);
|
|
}
|
|
|
|
void servo_to_initial_position(const int servo_id) {
|
|
dxlSetGoalPosition(servo_id, DXL_MID_MOTOR_INITIAL_POSITION);
|
|
}
|
|
|
|
void servos_to_initial_position() {
|
|
for(int i = 0; i < SERVO_COUNT; i++) {
|
|
dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
|
|
}
|
|
}
|
|
|
|
int servo_moving(const int servo_id) {
|
|
return dxlGetMoving(servo_id);
|
|
}
|
|
|
|
int servos_moving() {
|
|
int counter = 0;
|
|
for(int i = 0; i < SERVO_COUNT; i++) {
|
|
counter += dxlGetMoving(servos[i].id);
|
|
}
|
|
|
|
if(counter == SERVO_COUNT) {
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
void servos_in_position_wait() {
|
|
while(servos_moving() == 1);
|
|
}
|
|
|
|
void servo_in_position_wait(const int servo_id) {
|
|
while(dxlGetMoving(servo_id));
|
|
}
|
|
|
|
|
|
int servo_set_speed_rpm(const int servo_id, const int rpm) {
|
|
dxlSetGoalSpeed(servo_id, rpm);
|
|
return rpm;
|
|
}
|
|
|
|
void servo_move_to_angle(const int servo_id, const int deg) {
|
|
dxlSetGoalPosition(servo_id, angle_to_dxl_servo_position(deg));
|
|
}
|
|
|
|
void map_angle_to_servo_with_initial_position(const int servo_id, const int deg) {
|
|
dxlSetGoalPosition(servo_id, servos[servo_id -1].initial_position + servos[servo_id - 1].joint_orientation * angle_to_dxl_servo_position(deg));
|
|
}
|
|
|
|
void servos_move_to_angle(const int deg) {
|
|
for(int i = 0; i < SERVO_COUNT; i++) {
|
|
dxlSetGoalPosition(servos[i].id, angle_to_dxl_servo_position(deg));
|
|
}
|
|
}
|
|
|
|
int angle_to_dxl_servo_position(const int deg) {
|
|
return deg / ANGLE_PER_DIGIT_RATIO;
|
|
}
|
|
|
|
int rpm_to_dxl_servo_speed(const int rpm) {
|
|
return rpm / RPM_TO_SPEED_RATIO;
|
|
}
|
|
|
|
void map_roll_to_servos_and_move(const int roll){
|
|
dxlSetGoalPosition(servos[0].id, servos[0].joint_orientation * angle_to_dxl_servo_position(roll));
|
|
dxlSetGoalPosition(servos[2].id, servos[2].joint_orientation * angle_to_dxl_servo_position(roll));
|
|
dxlSetGoalPosition(servos[4].id, servos[4].joint_orientation * angle_to_dxl_servo_position(roll));
|
|
delay(1);
|
|
}
|
|
|
|
void map_pitch_to_servos_and_move(const int pitch){
|
|
dxlSetGoalPosition(servos[1].id, servos[1].joint_orientation * angle_to_dxl_servo_position(pitch));
|
|
dxlSetGoalPosition(servos[3].id, servos[3].joint_orientation * angle_to_dxl_servo_position(pitch));
|
|
delay(1);
|
|
}
|
|
|
|
void map_roll_to_servos_with_initial_position_and_move(const int roll){
|
|
dxlSetGoalPosition(servos[0].id, servos[0].initial_position + (servos[0].joint_orientation * angle_to_dxl_servo_position(roll)));
|
|
dxlSetGoalPosition(servos[2].id, servos[2].initial_position + (servos[2].joint_orientation * angle_to_dxl_servo_position(roll)));
|
|
dxlSetGoalPosition(servos[4].id, servos[4].initial_position + (servos[4].joint_orientation * angle_to_dxl_servo_position(roll)));
|
|
delay(1);
|
|
}
|
|
|
|
void map_pitch_to_servos_with_initial_position_and_move(const int pitch){
|
|
dxlSetGoalPosition(servos[1].id, servos[1].initial_position + (servos[1].joint_orientation * angle_to_dxl_servo_position(pitch)));
|
|
dxlSetGoalPosition(servos[3].id, servos[3].initial_position + (servos[3].joint_orientation * angle_to_dxl_servo_position(pitch)));
|
|
delay(1);
|
|
}
|
|
|
|
void map_servos_by_weights_with_initial_position_and_move(const int roll, const int pitch){
|
|
|
|
static unsigned long millis_add;
|
|
#define MILLISADD_MAX 100
|
|
millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
|
|
//Serial.print(mood.shakiness);
|
|
//Serial.print(", ");
|
|
//Serial.println(millis_add);
|
|
|
|
|
|
for (uint8_t i=0;i<=SERVO_COUNT;i++) {
|
|
float pnoise=PerlinNoise2((millis()+millis_add)/1000.0,i*10,0.25,3); //x,y,persistance,octaves
|
|
int angle=0;
|
|
angle=pitch*matrix_weights[i][0]+roll*matrix_weights[i][1]+pnoise*matrix_weights[i][2];
|
|
dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle)));
|
|
}
|
|
delay(1);
|
|
}
|
|
|
|
|
|
int servo_calculate_torque_with_direction(const int servo_id) {
|
|
int current_torque = dxlGetTorque(servo_id);
|
|
int direction = current_torque & MOTOR_TORQUE_DIRECTION_MASK;
|
|
current_torque &= MOTOR_TORQUE_VALUE_MASK;
|
|
current_torque *= MOTOR_TORQUE_RATIO;
|
|
if(direction == 0) {
|
|
current_torque *= -1;
|
|
}
|
|
return current_torque;
|
|
}
|
|
|
|
void servos_collect_current_torque() {
|
|
for(int i = 0; i < SERVO_COUNT; i++) {
|
|
servos[i].current_torque = servo_calculate_torque_with_direction(servos[i].id);
|
|
}
|
|
}
|
|
|
|
void servos_print_current_torque() {
|
|
Serial.print("s_t=[");
|
|
Serial.print(servos[SERVO_COUNT - 1].current_torque);
|
|
for(int i=SERVO_COUNT - 2; i >= 0; i--) {
|
|
Serial.print(",");
|
|
Serial.print(servos[i].current_torque);
|
|
}
|
|
Serial.println("]");
|
|
}
|