449 lines
14 KiB
C++
449 lines
14 KiB
C++
/*
|
|
* servo-control.cpp
|
|
*
|
|
* Created on: 12.01.2021
|
|
* Author: frank
|
|
*/
|
|
|
|
#include "servo-control.hpp"
|
|
|
|
SimplexNoise sn;
|
|
|
|
|
|
int angle_to_dxl_servo_position(const int deg);
|
|
int rpm_to_dxl_servo_speed(const int rpm);
|
|
|
|
|
|
Mood mood;
|
|
|
|
#define WEIGHT_COUNT 6 //enter number of weights here for array size
|
|
float matrix_weights[SERVO_COUNT][WEIGHT_COUNT]; //mapping outputs, sensors/moods
|
|
#define W_PITCH 0
|
|
#define W_ROLL 1
|
|
#define W_NOISE 2
|
|
#define W_NOISESLOW 3
|
|
#define W_SIN 4
|
|
#define W_COS 5
|
|
String weightNames[] = {"Pitch","Roll","Noise","NoiseSlow","Sin","Cos"};
|
|
|
|
unsigned long last_overloaderror_millis;
|
|
bool overloaderror_flag=false;
|
|
|
|
|
|
dxl_servo servos[SERVO_COUNT];
|
|
|
|
|
|
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
|
|
//float map_mode=constrain(mapfloat(mood.shakiness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
|
|
|
|
float map_mode_noise=constrain(mapfloat(mood.wakefulness,0,0.2, 0,1),0,1);
|
|
float map_mode_rollpitch=constrain(mapfloat(mood.shakiness,0,0.5, 0,1),0,1);
|
|
float map_mode_slither=0;
|
|
float map_mode_sleeping=mood.loneliness;
|
|
|
|
|
|
|
|
|
|
|
|
float map_sum=map_mode_noise+map_mode_rollpitch+map_mode_slither+map_mode_sleeping;
|
|
map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
|
|
map_mode_rollpitch/=map_sum;
|
|
map_mode_slither/=map_sum;
|
|
map_mode_sleeping/=map_sum;
|
|
|
|
/*
|
|
Serial.print(map_mode_noise); Serial.print(",");
|
|
Serial.print(map_mode_rollpitch); Serial.print(",");
|
|
Serial.print(map_mode_slither); Serial.print(",");
|
|
Serial.print(map_mode_sleeping); Serial.println();
|
|
*/
|
|
|
|
//Mode Noise
|
|
matrix_weights[0][W_NOISE]=90.0 *map_mode_noise;
|
|
matrix_weights[0][W_NOISESLOW]=0.0 *map_mode_noise;
|
|
|
|
matrix_weights[1][W_NOISE]=70.0 *map_mode_noise;
|
|
matrix_weights[1][W_NOISESLOW]=20.0 *map_mode_noise;
|
|
|
|
matrix_weights[2][W_NOISE]=50.0 *map_mode_noise;
|
|
matrix_weights[2][W_NOISESLOW]=40.0 *map_mode_noise;
|
|
|
|
matrix_weights[3][W_NOISE]=20.0 *map_mode_noise;
|
|
matrix_weights[3][W_NOISESLOW]=70.0 *map_mode_noise;
|
|
|
|
matrix_weights[4][W_NOISE]=0.0 *map_mode_noise;
|
|
matrix_weights[4][W_NOISESLOW]=60.0 *map_mode_noise;
|
|
|
|
//Mode Roll Pitch
|
|
matrix_weights[0][W_ROLL]=1.0* map_mode_rollpitch;
|
|
matrix_weights[1][W_PITCH]=1.0* map_mode_rollpitch;
|
|
matrix_weights[2][W_ROLL]=1.0* map_mode_rollpitch;
|
|
matrix_weights[3][W_PITCH]=1.0* map_mode_rollpitch;
|
|
matrix_weights[4][W_ROLL]=1.0* map_mode_rollpitch;
|
|
|
|
//Mode Slither
|
|
matrix_weights[0][W_COS]=30 *map_mode_slither;
|
|
matrix_weights[1][W_SIN]=30 *map_mode_slither;
|
|
matrix_weights[2][W_COS]=-30 *map_mode_slither;
|
|
matrix_weights[3][W_SIN]=-30 *map_mode_slither;
|
|
matrix_weights[4][W_COS]=30 *map_mode_slither;
|
|
|
|
//Mode Sleeping
|
|
matrix_weights[0][W_NOISESLOW]=20 *map_mode_sleeping;
|
|
matrix_weights[1][W_NOISESLOW]=20 *map_mode_sleeping;
|
|
matrix_weights[2][W_NOISESLOW]=30 *map_mode_sleeping;
|
|
matrix_weights[3][W_NOISESLOW]=15 *map_mode_sleeping;
|
|
matrix_weights[4][W_NOISESLOW]=10 *map_mode_sleeping;
|
|
|
|
/*
|
|
float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.1, 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;
|
|
matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1);
|
|
}
|
|
*/
|
|
}
|
|
|
|
|
|
bool trasmitMatrixWeightsHeaderFlag=false;
|
|
void transmit_matrix_weights() {
|
|
if (!trasmitMatrixWeightsHeaderFlag) { //transmit header only one time
|
|
trasmitMatrixWeightsHeaderFlag=true;
|
|
Serial.print("mwheader"); //matrixweight header
|
|
for (int i=0;i<WEIGHT_COUNT;i++) {
|
|
Serial.print(",");
|
|
Serial.print(weightNames[i]);
|
|
}
|
|
Serial.println();
|
|
}
|
|
|
|
Serial.print("mw"); //matrixweights
|
|
for (int s=0;s<SERVO_COUNT;s++) {
|
|
for (int w=0;w<WEIGHT_COUNT;w++) {
|
|
Serial.print(",");
|
|
Serial.print(matrix_weights[s][w]);
|
|
}
|
|
}
|
|
Serial.println();
|
|
|
|
}
|
|
|
|
|
|
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,servos[i].torque_limit);
|
|
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);
|
|
float vsin=sin((millis()+millis_add)/1000.0);
|
|
float vcos=cos((millis()+millis_add)/1000.0);
|
|
|
|
//unsigned long blastart=micros();
|
|
for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
|
//unsigned long blastart=micros();
|
|
//float pnoise=PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves
|
|
//float pnoisesslow=0;//PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves
|
|
//Serial.print("perlin micros="); Serial.println(micros()-blastart);
|
|
|
|
|
|
float pnoise=sn.noise((millis()+millis_add)/5000.0,i*10); //simplexnoise -1 to 1
|
|
float pnoiseslow=sn.noise((millis()+millis_add)/50000.0,i*10); //simplexnoise -1 to 1
|
|
|
|
|
|
int angle=0;
|
|
angle=
|
|
pitch*matrix_weights[i][W_PITCH]
|
|
+roll*matrix_weights[i][W_ROLL]
|
|
+pnoise*matrix_weights[i][W_NOISE]
|
|
+pnoiseslow*matrix_weights[i][W_NOISESLOW]
|
|
+vsin*matrix_weights[i][W_SIN]
|
|
+vcos*matrix_weights[i][W_COS];
|
|
|
|
dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle)));
|
|
|
|
|
|
}
|
|
//delay(1);
|
|
//Serial.print("setgoalposition micros="); Serial.println(micros()-blastart);
|
|
|
|
}
|
|
|
|
#define DELAY_OVERLOADERROR_RESET 1000
|
|
#define TORQUERECOVERTIME 5000
|
|
|
|
void set_servo_torque() {
|
|
float torque_multiplier=1; //value between 0 and 1
|
|
static float last_torque_multiplier=0;
|
|
torque_multiplier=constrain( mapfloat(millis()-last_overloaderror_millis, DELAY_OVERLOADERROR_RESET,TORQUERECOVERTIME, 0.0,1.0), 0,1);
|
|
if (last_torque_multiplier!=torque_multiplier) { //only if changed
|
|
Serial.print("torque,");
|
|
Serial.println(torque_multiplier);
|
|
for(int i = 0; i < SERVO_COUNT; i++) {
|
|
dxlSetRunningTorqueLimit(servos[i].id,servos[i].torque_limit* torque_multiplier);
|
|
}
|
|
last_torque_multiplier=torque_multiplier;
|
|
}
|
|
}
|
|
|
|
#define CHECK_BIT(var,pos) ((var) & (1<<(pos)))
|
|
void read_servos_positions()
|
|
{
|
|
int errors[SERVO_COUNT];
|
|
Serial.print("dxlgp");
|
|
for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
|
//Serial.print("servo "); Serial.print(i); Serial.print( "(id="); Serial.print(servos[i].id); Serial.print(")"); Serial.print("="); Serial.println(dxlGetPosition(servos[i].id));
|
|
|
|
Serial.print(",");
|
|
|
|
Serial.print(dxlGetPosition(servos[i].id));
|
|
errors[i]=ax12GetLastError();
|
|
|
|
|
|
|
|
}
|
|
Serial.println();
|
|
|
|
Serial.print("error");
|
|
for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
|
Serial.print(",");
|
|
Serial.print(errors[i]);
|
|
}
|
|
Serial.println();
|
|
|
|
|
|
|
|
for(int i = 0; i< SERVO_COUNT; i++) {
|
|
int errorcode=errors[i];
|
|
if (errorcode!=0) {
|
|
Serial.print("Error present on "); Serial.print(i); Serial.print("errorcode="); Serial.println(errorcode);
|
|
}
|
|
if (CHECK_BIT(errorcode,5)) { //overload error
|
|
if (!overloaderror_flag) { //error just appeared
|
|
last_overloaderror_millis=millis();
|
|
overloaderror_flag=true;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
if (millis()-last_overloaderror_millis>DELAY_OVERLOADERROR_RESET) { //wait before resetting
|
|
for(int i = 0; i< SERVO_COUNT; i++) {
|
|
int errorcode=errors[i];
|
|
if (CHECK_BIT(errorcode,5)) { //overload error
|
|
Serial.print("Init Servo "); Serial.println(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,0); //enable with no torque
|
|
dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
|
|
overloaderror_flag=false;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
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("]");
|
|
}
|