copy over behaviour template

This commit is contained in:
Philipp Kramer 2024-09-11 11:40:46 +02:00
parent 52e97a5cdd
commit 7f51d63380
10 changed files with 517 additions and 17 deletions

View file

@ -0,0 +1,114 @@
/*
===============================================================================
Ported the SimplexNoise algorithm from the C++ versions mentioned below
to a reuseable Arduino Library.
By Jordan Shaw / http://jordanshaw.com / 2017-02
A C++ port of a speed-improved simplex noise algorithm for 2D in Java.
Based on example code by Stefan Gustavson (stegu@itn.liu.se).
Optimisations by Peter Eastman (peastman@drizzle.stanford.edu).
Better rank ordering method by Stefan Gustavson in 2012.
C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com).
This could be speeded up even further, but it's useful as it is.
Version 2012-04-12
The original Java code was placed in the public domain by its original author,
Stefan Gustavson. You may use it as you see fit,
but attribution is appreciated.
Original gist url: https://gist.github.com/Slipyx/2372043
===============================================================================
*/
#include <math.h>
#include "Arduino.h"
#include "SimplexNoise.h"
// Private static member definitions
const double SimplexNoise::F2 = 0.5 * (sqrt( 3.0 ) - 1.0);
const double SimplexNoise::G2 = (3.0 - sqrt( 3.0 )) / 6.0;
const uint8_t SimplexNoise::p[256] = {
151,160,137,91,90,15,131,13,201,95,96,53,194,233,7,225,140,36,103,30,69,
142,8,99,37,240,21,10,23,190,6,148,247,120,234,75,0,26,197,62,94,252,219,
203,117,35,11,32,57,177,33,88,237,149,56,87,174,20,125,136,171,168,68,175,
74,165,71,134,139,48,27,166,77,146,158,231,83,111,229,122,60,211,133,230,
220,105,92,41,55,46,245,40,244,102,143,54,65,25,63,161,1,216,80,73,209,76,
132,187,208,89,18,169,200,196,135,130,116,188,159,86,164,100,109,198,173,
186,3,64,52,217,226,250,124,123,5,202,38,147,118,126,255,82,85,212,207,206,
59,227,47,16,58,17,182,189,28,42,223,183,170,213,119,248,152,2,44,154,163,
70,221,153,101,155,167,43,172,9,129,22,39,253,19,98,108,110,79,113,224,232,
178,185,112,104,218,246,97,228,251,34,242,193,238,210,144,12,191,179,162,
241,81,51,145,235,249,14,239,107,49,192,214,31,181,199,106,157,184,84,204,
176,115,121,50,45,127,4,150,254,138,236,205,93,222,114,67,29,24,72,243,141,
128,195,78,66,215,61,156,180
};
const Grad SimplexNoise::grad3[12] = {
Grad(1,1,0),Grad(-1,1,0),Grad(1,-1,0),Grad(-1,-1,0),Grad(1,0,1),
Grad(-1,0,1),Grad(1,0,-1),Grad(-1,0,-1),Grad(0,1,1),Grad(0,-1,1),
Grad(0,1,-1),Grad(0,-1,-1)
};
uint8_t SimplexNoise::perm[512] = {0};
uint8_t SimplexNoise::permMod12[512] = {0};
// Initialize permutaion arrays
void SimplexNoise::init() {
for ( uint16_t i = 0; i < 512; ++i ) {
perm[i] = p[i & 255];
permMod12[i] = static_cast<uint8_t>(perm[i] % 12);
}
delete[] &p; // lol what
}
// Fast floor
int32_t SimplexNoise::fastFloor( double x ) {
int32_t xi = static_cast<int32_t>(x);
return x < xi ? xi - 1 : xi;
}
double SimplexNoise::dot( const Grad& g, double x, double y ) {
return g.x * x + g.y * y;
}
// 2D SimplexNoise noise
double SimplexNoise::noise( double xin, double yin ) {
double s = (xin + yin) * F2;
int32_t i = fastFloor( xin + s );
int32_t j = fastFloor( yin + s );
double t = (i + j) * G2;
double x0 = xin - (i - t);
double y0 = yin - (j - t);
uint8_t i1 = 0, j1 = 1;
if ( x0 > y0 ) {
i1 = 1;
j1 = 0;
}
double x1 = x0 - i1 + G2;
double y1 = y0 - j1 + G2;
double x2 = x0 - 1.0 + 2.0 * G2;
double y2 = y0 - 1.0 + 2.0 * G2;
uint8_t ii = i & 255;
uint8_t jj = j & 255;
uint8_t gi0 = permMod12[ii + perm[jj]];
uint8_t gi1 = permMod12[ii + i1 + perm[jj + j1]];
uint8_t gi2 = permMod12[ii + 1 + perm[jj + 1]];
double n0 = 0.0;
double t0 = 0.5 - x0 * x0 - y0 * y0;
if ( t0 >= 0.0 ) {
t0 *= t0;
n0 = t0 * t0 * dot( grad3[gi0], x0, y0 );
}
double n1 = 0.0;
double t1 = 0.5 - x1 * x1 - y1 * y1;
if ( t1 >= 0.0 ) {
t1 *= t1;
n1 = t1 * t1 * dot( grad3[gi1], x1, y1 );
}
double n2 = 0.0;
double t2 = 0.5 - x2 * x2 - y2 * y2;
if ( t2 >= 0.0 ) {
t2 *= t2;
n2 = t2 * t2 * dot( grad3[gi2], x2, y2 );
}
return 70.0 * (n0 + n1 + n2);
}

View file

@ -0,0 +1,54 @@
/*
===============================================================================
Ported the SimplexNoise algorithm from the C++ versions mentioned below
to a reuseable Arduino Library.
By Jordan Shaw / http://jordanshaw.com / 2017-02
A C++ port of a speed-improved simplex noise algorithm for 2D in Java.
Based on example code by Stefan Gustavson (stegu@itn.liu.se).
Optimisations by Peter Eastman (peastman@drizzle.stanford.edu).
Better rank ordering method by Stefan Gustavson in 2012.
C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com).
This could be speeded up even further, but it's useful as it is.
Version 2012-04-12
The original Java code was placed in the public domain by its original author,
Stefan Gustavson. You may use it as you see fit,
but attribution is appreciated.
Original gist url: https://gist.github.com/Slipyx/2372043
===============================================================================
*/
#ifndef SimplexNoise_h
#define SimplexNoise_h
#include <math.h>
#include "Arduino.h"
class Grad {
public:
Grad( int8_t x, int8_t y, int8_t z ) : x(x), y(y), z(z) {}
int8_t x, y, z;
};
class SimplexNoise {
public:
// Initialize permutation arrays
static void init();
// 2D simplex noise
static double noise( double xin, double yin );
private:
static int32_t fastFloor( double x );
static double dot( const Grad& g, double x, double y );
static const double F2;
static const double G2;
static const Grad grad3[12];
static const uint8_t p[256];
static uint8_t perm[512];
static uint8_t permMod12[512];
};
#endif

View file

@ -0,0 +1,186 @@
#include "Arduino.h"
#include "behaviour.h"
SimplexNoise sn;
Mood mood;
float matrix_weights_main[SERVO_COUNT_MAIN][WEIGHT_COUNT]; //mapping outputs, sensors/moods
float matrix_weights_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT];
float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT];
void updateMatrixWeights(unsigned long millis, bool body_present,bool contact_main){
mood.loneliness=1.0;
static unsigned long last_weights_update=0;
if (millis-last_weights_update>WEIGHT_UPDATE_INTERVAL) {
last_weights_update=millis;
static float contact_main_persist=0;
if (contact_main) {
contact_main_persist=1.0;
}else{
if (contact_main_persist>0) {
contact_main_persist-=0.2*WEIGHT_UPDATE_INTERVAL/1000.0;
}else{
contact_main_persist=0;
}
}
//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;*/
bool state_bellyup=false;
bool state_map_mode_detached=false;
bool state_map_mode_attached=false;
float map_mode_rollpitch=0;
float map_mode_slither=0;
float map_mode_sleeping=0;
float contact_main_smooth=constrain(mapfloat(contact_main_persist,0.0,0.3, 0.0,1.0), 0.0,1.0);
float map_mode_noise=1.0-contact_main_smooth; //idle movement
float map_mode_scared=0.0+contact_main_smooth; //contracted position
float map_sum=map_mode_noise+map_mode_scared;
map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
map_mode_scared/=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_main[0][W_NOISE]=30.0 *map_mode_noise;
matrix_weights_main[0][W_NOISESLOW]=0.0 *map_mode_noise;
matrix_weights_main[1][W_NOISE]=30.0 *map_mode_noise;
matrix_weights_main[1][W_NOISESLOW]=20.0 *map_mode_noise;
matrix_weights_main[2][W_NOISE]=20.0 *map_mode_noise;
matrix_weights_main[2][W_NOISESLOW]=30.0 *map_mode_noise;
matrix_weights_main[0][W_LOOKDIRECTION]=30* map_mode_scared;
matrix_weights_main[1][W_LOOKDIRECTION]=30.0* map_mode_scared;
matrix_weights_main[2][W_LOOKDIRECTION]=30.0* map_mode_scared;
//Mode Roll Pitch
matrix_weights_main[0][W_ROLL]=1.0* map_mode_rollpitch;
matrix_weights_main[1][W_PITCH]=1.0* map_mode_rollpitch;
matrix_weights_main[2][W_ROLL]=1.0* map_mode_rollpitch;
//Mode Slither
matrix_weights_main[0][W_COS]=30 *map_mode_slither;
matrix_weights_main[1][W_SIN]=30 *map_mode_slither;
matrix_weights_main[2][W_COS]=-30 *map_mode_slither;
//Mode Sleeping
matrix_weights_main[0][W_NOISESLOW]=20 *map_mode_sleeping;
matrix_weights_main[1][W_NOISESLOW]=20 *map_mode_sleeping;
matrix_weights_main[2][W_NOISESLOW]=30 *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);
}
*/
}
}
void updateServosByWeights(unsigned long millis){
static unsigned long last_servoweights_update=0;
if (millis-last_servoweights_update>SERVOWEIGHT_UPDATE_INTERVAL)
{
last_servoweights_update=millis;
static unsigned long millis_add;
#define MILLISADD_MAX 100
millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
float pitch=0;
float roll=0;
float vsin=sin((millis()+millis_add)/1000.0);
float vcos=cos((millis()+millis_add)/1000.0);
for (uint8_t i=0;i<SERVO_COUNT;i++) {
uint8_t servoPosInMatrix=0;
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
float angle=0;
float lookdirection=(servos[i].angle>=0)?1:-1; //if angle positive, 1, else 0
if (i<SERVO_COUNT_MAIN) { //Main
/*if (i==0){
Serial.print("servoangle:");Serial.println(servos[i].angle);
Serial.print("lookdirection:");Serial.println(lookdirection);
Serial.print("weight:");Serial.println(matrix_weights_main[i][W_LOOKDIRECTION]);
}*/
servoPosInMatrix=i;
angle=
pitch*matrix_weights_main[servoPosInMatrix][W_PITCH]
+roll*matrix_weights_main[servoPosInMatrix][W_ROLL]
+pnoise*matrix_weights_main[servoPosInMatrix][W_NOISE]
+pnoiseslow*matrix_weights_main[servoPosInMatrix][W_NOISESLOW]
+vsin*matrix_weights_main[servoPosInMatrix][W_SIN]
+vcos*matrix_weights_main[servoPosInMatrix][W_COS]
+lookdirection*matrix_weights_main[servoPosInMatrix][W_LOOKDIRECTION];
}else if(i<SERVO_COUNT_MAIN+SERVO_COUNT_PERIPHERAL) { //Peripheral Left
servoPosInMatrix=i-3;
}else if(i<SERVO_COUNT_MAIN+2*SERVO_COUNT_PERIPHERAL) { //Peripheral Right
servoPosInMatrix=i-2*3;
}
servos[i].angle=angle;
}
}
}
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}

View file

@ -0,0 +1,43 @@
#ifndef BEHAVIOUR_H_
#define BEHAVIOUR_H_
#include "SimplexNoise.h" //Library from https://github.com/jshaw/SimplexNoise
#include "definitions.h"
#include "servo.h"
extern dxl_servo servos[SERVO_COUNT];
#define SERVO_COUNT_MAIN 3
#define SERVO_COUNT_PERIPHERAL 3
#define WEIGHT_COUNT 7 //enter number of weights here for array size
#define WEIGHT_UPDATE_INTERVAL 100
#define SERVOWEIGHT_UPDATE_INTERVAL SERVO_UPDATE_INTERVAL
#define W_PITCH 0
#define W_ROLL 1
#define W_NOISE 2
#define W_NOISESLOW 3
#define W_SIN 4
#define W_COS 5
#define W_LOOKDIRECTION 6
struct Mood{
float shakiness;
float wakefulness;
float loneliness;
};
extern Mood mood;
void updateMatrixWeights(unsigned long millis,bool body_present, bool contact_main);
void updateServosByWeights(unsigned long millis);
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
#endif

View file

@ -0,0 +1,57 @@
#include "Arduino.h"
#include "bodytemp.h"
Adafruit_MLX90614 mlx = Adafruit_MLX90614();
double bodytemp_ambient=0;
double bodytemp_object=0;
bool body_present=false;
const double bodytemp_activate_temperature=30; //°C min object temperature to detect human
const double bodytemp_activate_mindiff_ambient=3; //°C object temperature has to be at least this amount above ambient
const double bodytemp_deactivate_maxdiff_ambient=2; //°C maximum difference between object and ambient temperature to not detect human anymore
const unsigned long bodytemp_deacitvate_mintime=1000; //minimum time in ms between activate and deactivate
void initBodytemp() {
Wire.begin(PIN_SDA,PIN_SCL);
if (!mlx.begin(0x5A,&Wire)) {
Serial.println("Error connecting to MLX sensor. Check wiring.");
while (1);
};
}
void printBodytempDebug() {
Serial.print("min:20,max:40,");
Serial.print("Ambient:");Serial.print(bodytemp_ambient);
Serial.print(",Object:");Serial.print(bodytemp_object);
Serial.print(",Presence:");Serial.print(body_present?39:21);
}
bool checkBodypresence(unsigned long millis) {
static unsigned long last_read_bodytemp=0;
if (millis-last_read_bodytemp > BODYTEMP_READ_MIN_INTERVAL)
{
last_read_bodytemp = millis;
bodytemp_ambient = mlx.readAmbientTempC();
bodytemp_object = mlx.readObjectTempC();
static unsigned long last_change_bodypresent=0;
if (millis-last_change_bodypresent > bodytemp_deacitvate_mintime) {}
if (!body_present) { //is currently not present
if (bodytemp_object >= bodytemp_activate_temperature && (bodytemp_object-bodytemp_ambient) >= bodytemp_activate_mindiff_ambient) {
body_present=true;
last_change_bodypresent=millis;
}
}else{ //was present last time
if((bodytemp_object-bodytemp_ambient) <= bodytemp_deactivate_maxdiff_ambient) {
body_present=false;
last_change_bodypresent=millis;
}
}
}
return body_present;
}

View file

@ -0,0 +1,13 @@
#ifndef BODYTEMP_H_
#define BODYTEMP_H_
#include <Adafruit_MLX90614.h> //Adafruit MLX90614 Library by Adafruit 2.1.5
#include "definitions.h"
#define BODYTEMP_READ_MIN_INTERVAL 500
void initBodytemp();
void printBodytempDebug() ;
bool checkBodypresence(unsigned long millis);
#endif

View file

@ -22,7 +22,7 @@
#define PIN_OUT7 8 //PWM #define PIN_OUT7 8 //PWM
#define PIN_OUT8 9 //PWM #define PIN_OUT8 9 //PWM
#define PIN_BUTTON 23 #define PIN_BUTTON BOARD_BUTTON_PIN //BOARD_BUTTON_PIN = 23
#define PIN_PUMP1 PIN_OUT5 #define PIN_PUMP1 PIN_OUT5
#define PIN_PUMP2 PIN_OUT7 #define PIN_PUMP2 PIN_OUT7
@ -32,6 +32,11 @@
#define PIN_VIBRATION PIN_OUT2 #define PIN_VIBRATION PIN_OUT2
#define PIN_SDA 25
#define PIN_SCL 24
#define PIN_LED BOARD_LED_PIN //BOARD_LED_PIN=14
#endif #endif

View file

@ -1,6 +1,8 @@
#include "definitions.h" #include "definitions.h"
#include "vacuum.h" #include "vacuum.h"
#include "servo.h" #include "servo.h"
#include "bodytemp.h"
#include "behaviour.h"
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
@ -9,11 +11,16 @@ void setup() {
analogWrite(PIN_VIBRATION,0); analogWrite(PIN_VIBRATION,0);
pinMode(PIN_BUTTON,INPUT_PULLDOWN); pinMode(PIN_BUTTON,INPUT_PULLDOWN);
pinMode(PIN_LED,OUTPUT);
digitalWrite(PIN_LED,HIGH); //LOW=Light, HIGH=Off
initVacuum(); initVacuum();
initBodytemp();
pinMode(PIN_INPUTARM1,INPUT); pinMode(PIN_INPUTARM1,INPUT);
pinMode(PIN_INPUTARM2,INPUT);
pinMode(PIN_INPUTARM3,INPUT);
pinMode(PIN_INPUTAUX,INPUT); pinMode(PIN_INPUTAUX,INPUT);
@ -23,20 +30,34 @@ void setup() {
void loop() { void loop() {
unsigned long loopmillis=millis(); unsigned long loopmillis=millis();
bool vac=digitalRead(PIN_BUTTON); //Temporary Vacuum Button
setVacuum(vac); bool body_present=checkBodypresence(loopmillis);
digitalWrite(PIN_LED,!body_present);
loopVacuum(loopmillis); loopVacuum(loopmillis);
bool buttonstate=digitalRead(PIN_BUTTON); //Temporary Vacuum Button. HIGH=Pressed
if (buttonstate) { //Button pressed
setVacuum(false); //release
}else{
setVacuum(body_present);
}
bool contact_main=digitalRead(PIN_INPUTARM1); //Sensor on tip of arm
updateMatrixWeights(loopmillis,body_present,contact_main);
updateServosByWeights(loopmillis);
loopServos(loopmillis); loopServos(loopmillis);
//Print Debug Information //Print Debug Information
static unsigned long last_print=0; static unsigned long last_print=0;
if (loopmillis - last_print >100) { if (loopmillis - last_print >100) {
@ -45,9 +66,11 @@ void loop() {
//Serial.print(" Vac="); Serial.print(vac); //Serial.print(" Vac="); Serial.print(vac);
printServoDebug(); printServoDebug();
Serial.println(); Serial.println();
//printBodytempDebug();
Serial.println(); Serial.println();
} }

View file

@ -3,10 +3,10 @@
using namespace DYNAMIXEL; using namespace DYNAMIXEL;
#define SERVO_COUNT 3
const uint8_t DXL_ID = 1; const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 2.0; const float DXL_PROTOCOL_VERSION = 2.0;
dxl_servo servos[SERVO_COUNT]; dxl_servo servos[SERVO_COUNT];
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
@ -100,18 +100,15 @@ void initServos()
void loopServos(unsigned long millis) { void loopServos(unsigned long millis) {
servos[0].angle=sin(millis/1000.0/1.0)*20; /*servos[0].angle=sin(millis/1000.0/1.0)*20;
servos[1].angle=cos(millis/1000.0/1.0)*20; servos[1].angle=cos(millis/1000.0/1.0)*20;
servos[2].angle=sin(millis/1000.0/1.0)*20; servos[2].angle=sin(millis/1000.0/1.0)*20;*/
static uint8_t servoUpdateI=0; //only one servo per function call static uint8_t servoUpdateI=0; //only one servo per function call
static unsigned long last_servo_update=0; static unsigned long last_servo_update=0;
if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT){ if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT){
last_servo_update=millis; last_servo_update=millis;
//for (uint8_t i=0;i<SERVO_COUNT;i++) {
//dxl.setGoalPosition(servos[i].id, servos[i].offset_angle+servos[i].angle*servos[i].orientation, UNIT_DEGREE);
//}
dxl.setGoalPosition(servos[servoUpdateI].id, servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation, UNIT_DEGREE); dxl.setGoalPosition(servos[servoUpdateI].id, servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation, UNIT_DEGREE); //asdf
servoUpdateI++; servoUpdateI++;
servoUpdateI%=SERVO_COUNT; servoUpdateI%=SERVO_COUNT;
} }
@ -120,7 +117,8 @@ void loopServos(unsigned long millis) {
void printServoDebug() { void printServoDebug() {
for (uint8_t i=0;i<SERVO_COUNT;i++) { for (uint8_t i=0;i<SERVO_COUNT;i++) {
Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(dxl.getPresentPosition(servos[i].id, UNIT_DEGREE)); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle); //Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(dxl.getPresentPosition(servos[i].id, UNIT_DEGREE)); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(servos[i].offset_angle+servos[i].angle);
Serial.println(); Serial.println();
} }
} }

View file

@ -18,6 +18,11 @@
#define SERVO_UPDATE_INTERVAL 1000/50 #define SERVO_UPDATE_INTERVAL 1000/50
#define SERVO_COUNT 3
struct dxl_servo { struct dxl_servo {
int id; int id;
//int initial_speed; //int initial_speed;
@ -31,6 +36,8 @@ struct dxl_servo {
uint16_t modelnumber; uint16_t modelnumber;
}; };
void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid); void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid);
void scanDynamixel(); void scanDynamixel();
void changeID(uint8_t present_id, uint8_t new_id); void changeID(uint8_t present_id, uint8_t new_id);