diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index 1ddd2e2..df7ffa5 100644 --- a/examples/firmware/firmware.ino +++ b/examples/firmware/firmware.ino @@ -122,6 +122,8 @@ void loop(){ break; case 'P': alvik.setPositionLeft(value); + ack_required=MOVEMENT_LEFT; + ack_check=true; break; case 'Z': alvik.resetPositionLeft(value); @@ -136,6 +138,8 @@ void loop(){ break; case 'P': alvik.setPositionRight(value); + ack_required=MOVEMENT_RIGHT; + ack_check=true; break; case 'Z': alvik.resetPositionRight(value); @@ -149,6 +153,8 @@ void loop(){ packeter.unpacketC2F(code,position_left, position_right); alvik.disableKinematicsMovement(); alvik.setPosition(position_left, position_right); + ack_required=MOVEMENT_POSITION; + ack_check=true; break; @@ -280,13 +286,22 @@ void loop(){ msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]); alvik.serial->write(packeter.msg,msg_size); } - if (ack_check && alvik.isTargetReached()){ + if (ack_check && (alvik.isTargetReached() || alvik.isPositionReached() || alvik.isPositionLeftReached() || alvik.isPositionRightReached())){ if (ack_required == MOVEMENT_ROTATE){ msg_size = packeter.packetC1B('x', 'R'); } if (ack_required == MOVEMENT_MOVE){ msg_size = packeter.packetC1B('x', 'M'); } + if (ack_required == MOVEMENT_POSITION){ + msg_size = packeter.packetC1B('x', 'P'); + } + if (ack_required == MOVEMENT_LEFT){ + msg_size = packeter.packetC1B('x', 'P'); + } + if (ack_required == MOVEMENT_RIGHT){ + msg_size = packeter.packetC1B('x', 'P'); + } } else{ msg_size = packeter.packetC1B('x', 0); diff --git a/library.properties b/library.properties index b221ab0..a03415b 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Arduino_AlvikCarrier -version=1.0.2 +version=1.0.3 author=Arduino, Giovanni di Dio Bruno, Lucio Rossi maintainer=Arduino sentence=Library and firmware for Arduino Alvik Carrier board diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index ac7cc05..139089a 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -362,6 +362,10 @@ float Arduino_AlvikCarrier::getPositionLeft(){ return motor_control_left->getPosition(); } +bool Arduino_AlvikCarrier::isPositionLeftReached(){ + return motor_control_left->isPositionReached(); +} + void Arduino_AlvikCarrier::setPositionRight(const float degrees){ motor_control_right->setPosition(degrees); } @@ -370,6 +374,10 @@ float Arduino_AlvikCarrier::getPositionRight(){ return motor_control_right->getPosition(); } +bool Arduino_AlvikCarrier::isPositionRightReached(){ + return motor_control_right->isPositionReached(); +} + void Arduino_AlvikCarrier::setPosition(const float left_deg, const float right_deg){ setPositionLeft(left_deg); setPositionRight(right_deg); @@ -380,6 +388,10 @@ void Arduino_AlvikCarrier::getPosition(float & left_deg, float & right_deg){ right_deg = getPositionRight(); } +bool Arduino_AlvikCarrier::isPositionReached(){ + return isPositionLeftReached() && isPositionRightReached(); +} + void Arduino_AlvikCarrier::resetPositionLeft(const float initial_position){ motor_control_left->resetPosition(initial_position); } diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 2388075..9d683aa 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -172,10 +172,13 @@ class Arduino_AlvikCarrier{ void setKPidRight(const float kp, const float ki, const float kd); // set PID parameters for right wheel void setPositionLeft(const float degrees); // set position in degrees on left wheel float getPositionLeft(); // get left wheel position in degrees + bool isPositionLeftReached(); // return true if left wheel position is reached void setPositionRight(const float degrees); // set position in degrees on right wheel float getPositionRight(); // get right wheel position in degrees + bool isPositionRightReached(); // return true if right wheel position is reached void setPosition(const float left_deg, const float right_deg); // set positions on both wheels void getPosition(float & left_deg, float & right_deg); // get both wheels position + bool isPositionReached(); // return true if both wheels position are reached void resetPositionLeft(const float initial_position=0.0); // reset/set value of position for left wheel void resetPositionRight(const float initial_position=0.0); // reset/set value of position for right wheel void disablePositionControlLeft(); // disable the position control on left wheel diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index bede86c..54540ab 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -37,6 +37,7 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO; #define POSITION_KD_DEFAULT 0.0001 #define POSITION_CONTROL_PERIOD 0.02 #define POSITION_MAX_SPEED 30.0 +#define POSITION_THRESHOLD 10 @@ -58,6 +59,9 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO; #define MOVEMENT_DISABLED 0 #define MOVEMENT_ROTATE 1 #define MOVEMENT_MOVE 2 +#define MOVEMENT_LEFT 3 +#define MOVEMENT_RIGHT 4 +#define MOVEMENT_POSITION 5 #define LIFT_THRESHOLD 80 @@ -88,7 +92,7 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ); // Library version #define VERSION_BYTE_HIGH 1 #define VERSION_BYTE_MID 0 -#define VERSION_BYTE_LOW 2 +#define VERSION_BYTE_LOW 3 diff --git a/src/motor_control/motor_control.cpp b/src/motor_control/motor_control.cpp index 13aa831..843061f 100644 --- a/src/motor_control/motor_control.cpp +++ b/src/motor_control/motor_control.cpp @@ -35,6 +35,7 @@ MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp pos_controller_period = _pos_controller_period; pos_max_velocity = _pos_max_velocity; position_control_enabled = false; + is_position_reached = false; @@ -168,6 +169,9 @@ void MotorControl::update(){ if (position_control_enabled){ pos_pid->update(position); setRPM(round(pos_pid->getControlOutput()/10.0)*10); + if (abs(pos_pid->getError())setReference(degree); enablePositionControl(); + is_position_reached = false; +} + +bool MotorControl::isPositionReached(){ + return is_position_reached; } diff --git a/src/motor_control/motor_control.h b/src/motor_control/motor_control.h index e3ffa5a..3adffb4 100644 --- a/src/motor_control/motor_control.h +++ b/src/motor_control/motor_control.h @@ -43,6 +43,7 @@ class MotorControl{ float pos_controller_period; float pos_max_velocity; bool position_control_enabled; + bool is_position_reached; float position; float angle; @@ -112,6 +113,7 @@ class MotorControl{ void setPosition(const float degree); // set the reference for position control float getPosition(); // get the actual angle in degrees of motor void resetPosition(const float p0=0.0); // reset/set the position value + bool isPositionReached(); float getError();