...
 
Commits (5)
......@@ -11,7 +11,7 @@
#include <library/routine_slave/Etapes.h>
Movement::Movement(float32_t x, float32_t y,
uint16_t maskEvitement, uint16_t id) {
uint16_t id, uint16_t maskEvitement) {
x_ = x;
y_ = y;
maskEvitement_ = maskEvitement;
......@@ -23,7 +23,7 @@ uint8_t Movement::execute(){
}
RelativeMovement::RelativeMovement(float32_t x, float32_t y,
uint16_t maskEvitement, uint16_t id) {
uint16_t id, uint16_t maskEvitement) {
x_ = x;
y_ = y;
maskEvitement_ = maskEvitement;
......@@ -34,7 +34,7 @@ uint8_t RelativeMovement::execute(){
return robot_->goToRelativePosition((point_t){x_,y_});
}
Rotation::Rotation(float32_t angleToGo, uint16_t maskEvitement, uint16_t id) {
Rotation::Rotation(float32_t angleToGo, uint16_t id, uint16_t maskEvitement) {
angleToGo_ = angleToGo;
maskEvitement_ = maskEvitement;
id_ = id;
......@@ -46,7 +46,7 @@ uint8_t Rotation::execute(){
}
RelativeRotation::RelativeRotation(float32_t angleToGo,
uint16_t maskEvitement, uint16_t id) {
uint16_t id, uint16_t maskEvitement) {
angleToGo_ = angleToGo;
maskEvitement_ = maskEvitement;
id_ = id;
......@@ -58,7 +58,7 @@ uint8_t RelativeRotation::execute(){
}
PositionArrived::PositionArrived(uint16_t maskEvitement, uint16_t id) {
PositionArrived::PositionArrived(uint16_t id, uint16_t maskEvitement) {
maskEvitement_ = maskEvitement;
id_ = id;
}
......@@ -70,7 +70,7 @@ uint8_t PositionArrived::execute(){
return 2;
}
RotationArrived::RotationArrived(uint16_t maskEvitement, uint16_t id) {
RotationArrived::RotationArrived(uint16_t id, uint16_t maskEvitement) {
maskEvitement_ = maskEvitement;
id_ = id;
}
......@@ -82,7 +82,7 @@ uint8_t RotationArrived::execute(){
return 2;
}
Action::Action(EnumAction action, uint16_t maskEvitement, uint16_t id) {
Action::Action(EnumAction action, uint16_t id, uint16_t maskEvitement) {
action_ = action;
maskEvitement_ = maskEvitement;
id_ = id;
......@@ -156,7 +156,7 @@ uint8_t Action::execute(){
}
}
Delay::Delay(uint16_t delayToWait, uint16_t maskEvitement, uint16_t id) {
Delay::Delay(uint16_t delayToWait, uint16_t id, uint16_t maskEvitement) {
delayToWait_ = delayToWait;
id_ = id;
}
......
......@@ -12,12 +12,12 @@
/* Méthodes ------------------------------------------------------------------*/
void Routine::initRoutineBlue(){
addMovement(500.0f, 0.0f, 0, MASK_CAPTEUR_NONE);
addPositionArrived(0,MASK_CAPTEUR_NONE);
addMovement(500.0f, 500.0f, 0, MASK_CAPTEUR_NONE);
addPositionArrived(0,MASK_CAPTEUR_NONE);
addMovement(0.0f, 500.0f, 0, MASK_CAPTEUR_NONE);
addPositionArrived(0,MASK_CAPTEUR_NONE);
addMovement(0.0f, 0.0f, 0, MASK_CAPTEUR_NONE);
addPositionArrived(0,MASK_CAPTEUR_NONE);
addMovement(500.0f, 0.0f, 0, MASK_CAPTEUR_ALL);
addPositionArrived(0,MASK_CAPTEUR_ALL);
// addMovement(500.0f, 500.0f, 0, MASK_CAPTEUR_NONE);
// addPositionArrived(0,MASK_CAPTEUR_NONE);
// addMovement(0.0f, 500.0f, 0, MASK_CAPTEUR_NONE);
// addPositionArrived(0,MASK_CAPTEUR_NONE);
// addMovement(0.0f, 0.0f, 0, MASK_CAPTEUR_NONE);
// addPositionArrived(0,MASK_CAPTEUR_NONE);
}
Subproject commit 0c36426638dc78345491abac0ae99c0967b8c1b2
Subproject commit a0eb0023881ac92f4548c4dcc22b0f2ea9bb5836
......@@ -62,9 +62,9 @@ Robot::Robot() {
_uartoDrive = new Uart(&huart8);
// Asserv Vitesse G
_asservVitG = new ODriveVit(1,_uartoDrive,1);
_asservVitG = new ODriveVit(0,_uartoDrive,1);
// Asserv Vitesse D
_asservVitD = new ODriveVit(0,_uartoDrive,0);
_asservVitD = new ODriveVit(1,_uartoDrive,0);
// Codeuse Roue G
_codRoueG = new Encodeur(&htim3,TIM_CHANNEL_1,PAS_CODEUSE_ROUE);
......@@ -135,6 +135,8 @@ void Robot::launch() {
_deplacement->initPosition((point_t){0,0});
_odometrie->setAngle(0);
_asservVitG->setVitesse(0);
_asservVitD->setVitesse(0);
uint16_t ct_asserv=0;
uint16_t ct_comm=0;
......@@ -150,6 +152,7 @@ void Robot::launch() {
_uartDynamixel->update();
_uartoDrive->update();
if(_flagUpdate > 0){
_odometrie->update();
ct_asserv++;
......@@ -158,9 +161,6 @@ void Robot::launch() {
ct_routine++;
_flagUpdate = 0;
_asservVitG->setVitesse(0);
_asservVitD->setVitesse(0);
if(ct_comm >= INTERVAL_COMM){
_dynamixelMgt->update();
_robotCom->update();
......@@ -236,16 +236,25 @@ void Robot::calibrationAngle(){
void Robot::calibrationDistance(){
uint8_t step = 0;
uint8_t ct_asserv = 0;
uint8_t ct_deplacement = 0;
uint32_t ct_asserv = 0;
uint32_t ct_deplacement = 0;
uint8_t ct_calib = 0;
bool calibration = 0;
uint32_t timer=0;
bool wait=false;
while(!calibration){
_uartoDrive->update();
if(_flagUpdate > 0){
_flagUpdate = 0;
_odometrie->update();
ct_asserv++;
ct_deplacement++;
if(wait){
timer++;
}
if(ct_asserv >= INTERVAL_ASSERV){
ct_asserv = 0;
_asservPos->update();
......@@ -256,31 +265,47 @@ void Robot::calibrationDistance(){
}
}
if (step == 0) {
_deplacement->goToPosition((point_t){0,1120});
_deplacement->goToPosition((point_t){545,0});
step++;
}
if (step == 1) {
if (_asservPos->isGivenDestinationReached((point_t){0,1120})){
if (_asservPos->isGivenDestinationReached((point_t){545,0})){
_asservPos->goToAngle(0);
_asservPos->update();
step++;
}
}
if (step == 2) {
if (step == 3) {
if (_asservPos->isAngleArrived()) {
_deplacement->goToPosition((point_t){0,0});
step++;
}
}
if (step == 3) {
if (step == 2) {
wait=true;
if (timer > 1000) {
timer = 0;
wait=false;
step++;
}
}
if (step == 4) {
if (_asservPos->isGivenDestinationReached((point_t){0,0})){
_asservPos->goToAngle(0);
_asservPos->update();
step++;
}
}
if (step == 4) {
if (ct_calib < 5){
if (step == 5) {
wait=true;
if (timer > 1000) {
timer = 0;
wait=false;
step++;
}
}
if (step == 6) {
if (ct_calib < 4){
ct_calib++;
step = 0;
} else {
......
......@@ -82,6 +82,7 @@ public:
uint32_t getGlobalClock();
void StopMovement();
void SendBlockedSignal(uint16_t detectionzone, uint16_t id);
void AddEtapeToRoutine(Etape *etape);
void StartGame();
......@@ -152,8 +153,6 @@ private:
//Dynamixel management
DynamixelMgt* _dynamixelMgt;
robotComSlave* _robotComm;
//Dynamixel
Dynamixel* _pinceGoldDroite;
Dynamixel* _pinceGoldGauche;
......
......@@ -165,6 +165,10 @@ void Robot::StopMovement(){
_deplacement->stop();
}
void Robot::SendBlockedSignal(uint16_t detectionzone, uint16_t id){
}
void Robot::AddEtapeToRoutine(Etape *etape) {
_routine->addEtape(etape);
}
......
......@@ -17,14 +17,14 @@
#define INTERVAL_ASSERV 10//en ms
#define INTERVAL_COMM 50//en ms
#define INTERVAL_DEPLACEMENT 50//en ms
#define INTERVAL_DEPLACEMENT 100//en ms
#define INTERVAL_ROUTINE 50//en ms
#define taillereg 65536
//Deplacement
#define MAX_ACCEL 10.0f
#define VMAX 500.0f
#define MAX_ACCEL 10.0f // mm/s^2
#define VMAX 800.0f // mm/s
#define NB_POINT_ACCEL (uint16_t)(VMAX/MAX_ACCEL)
#define RAMP_LENGTH NB_POINT_ACCEL*(NB_POINT_ACCEL-1)*MAX_ACCEL/2
......