1534 lines
35 KiB
C++
1534 lines
35 KiB
C++
//General
|
|
#include <SoftwareSerial.h>
|
|
#include <Wire.h>
|
|
#include <math.h>
|
|
//Motor
|
|
//#include <Servo.h>
|
|
#include <Servo.h>
|
|
#include <AccelStepper.h>
|
|
//Sensor
|
|
#include "ams_as5048b.h"
|
|
|
|
#define DATA_SIZE_IN_COMMAND 10 //9 for movej and 1 for eot
|
|
|
|
#define P_MAX_LOWER 192
|
|
#define P_MAX_UP 192
|
|
|
|
#define R1_ANGLE_OFFSET 234
|
|
|
|
#define R2_LENGTH 140
|
|
|
|
#define R2_CENTER_OFFSET_X 36 //from center to start of servo in x at home
|
|
#define R2_CENTER_OFFSET_Y 6 //from start of servo to center of R2
|
|
#define R2_HEIGHT_OFFSET 96//wie hoch ist R2 Achse über dem Boden - war 96
|
|
#define R1_ZERO_ANGLE 0
|
|
#define TIME_PER_DEG_SERVO 5 //ms
|
|
#define R1_UEBERSETZUNG 2
|
|
|
|
#define P_STEP_MULTIPLY 5 //was 3
|
|
|
|
#define Q_SIZE 12
|
|
|
|
#define MAX_ACCEL_P 400
|
|
#define MAX_ACCEL_R1 400
|
|
#define STEPPER_SPEED_SCALING 5
|
|
|
|
//scew correction
|
|
#define SCEW_RAD 130
|
|
#define ZY_SCEW_AT_P130 -1
|
|
#define ZY_SCEW_AT_M130 9
|
|
#define ZX_SCEW_AT_P130 6
|
|
#define ZX_SCEW_AT_M130 1
|
|
|
|
#define EOT1 0b11111110
|
|
#define EOT2 0b11111110
|
|
#define DONT_CHANGE 0b0111111111111111
|
|
|
|
typedef struct
|
|
{
|
|
byte signature;
|
|
byte priority;
|
|
byte type;
|
|
|
|
byte data[DATA_SIZE_IN_COMMAND];
|
|
} command;
|
|
|
|
typedef struct
|
|
{
|
|
int x;
|
|
int y;
|
|
int z;
|
|
|
|
}KartesianCoordinates;
|
|
|
|
|
|
typedef struct
|
|
{
|
|
double t1;
|
|
double r;
|
|
double z;
|
|
|
|
}PolarCoordinates;
|
|
|
|
typedef struct
|
|
{
|
|
int R1; //in degrees
|
|
int P; //in mm
|
|
int R2; //in degrees
|
|
uint16_t eef; //one or zero
|
|
|
|
}Pose;
|
|
|
|
//function definitions
|
|
Pose inverse(KartesianCoordinates, int pref=0);
|
|
bool endstop_reached();
|
|
bool sendStatus();
|
|
bool test_inverse();
|
|
bool initialization_pins();
|
|
bool initialization_drive();
|
|
bool initialization_communication();
|
|
bool test_inverse();
|
|
bool generate_subcommand();
|
|
bool moveJ(KartesianCoordinates);
|
|
bool moveC();
|
|
bool moveL();
|
|
bool checkPositionLegality();
|
|
PolarCoordinates transform(KartesianCoordinates);
|
|
KartesianCoordinates transform(PolarCoordinates);
|
|
double rad2grad(double);
|
|
Pose SI_to_Robot(Pose);
|
|
Pose inverse(PolarCoordinates, int);
|
|
Pose inverse(KartesianCoordinates, int);
|
|
bool command_done();
|
|
bool execute_subcommand();
|
|
bool getCommunication();
|
|
bool sendStatus();
|
|
bool present();
|
|
|
|
|
|
Pose currentPose;
|
|
|
|
//class handler for R1 Motor and R1 rotational Sensor
|
|
//partially extends the accelstepper library
|
|
class R1_Handler
|
|
{
|
|
private:
|
|
|
|
AccelStepper* R1_Motor;//step,dir
|
|
int spin_R1 = 0;
|
|
|
|
//spin = -1 is turning towards 0, spin = 1 is turning towards 1
|
|
//call this constantly whenever robot moves
|
|
//!TODO find better implementation of when to call this function
|
|
void setSpinAngle()
|
|
{
|
|
|
|
//remap read angle to robot angle
|
|
int currentAngle = posSensor.angleR(U_DEG,true)-R1_ANGLE_OFFSET;
|
|
if (currentAngle < 0)
|
|
currentAngle = currentAngle+360;
|
|
|
|
//if initial, only trigger, when over 180
|
|
if(spin_R1 == 0)
|
|
{
|
|
if(currentAngle > 180 && currentAngle < 240)
|
|
spin_R1 = 1;
|
|
}
|
|
else if(currentAngle > 180 && currentAngle < 240)
|
|
{
|
|
spin_R1 = 1;
|
|
}
|
|
else if(currentAngle < 180 && currentAngle > 120)
|
|
{
|
|
spin_R1 = -1;
|
|
}
|
|
|
|
}
|
|
public:
|
|
|
|
AMS_AS5048B posSensor; //sda , scl
|
|
|
|
R1_Handler(int type,int step, int dir)
|
|
{
|
|
R1_Motor = new AccelStepper(type,step,dir);//step,dir
|
|
}
|
|
|
|
void parameter(int maxSpeed, int Speed, int Acceleration)
|
|
{
|
|
R1_Motor->setMaxSpeed(maxSpeed); // Set Max Speed of Stepper (Slower to get better accuracy)
|
|
R1_Motor->setSpeed(Speed);
|
|
R1_Motor->setAcceleration(Acceleration); // Set Acceleration of Stepper
|
|
}
|
|
|
|
void setCurrentPosition(int curPos)
|
|
{
|
|
R1_Motor->setCurrentPosition(curPos);
|
|
}
|
|
|
|
void run()
|
|
{
|
|
R1_Motor->run();
|
|
//ACTIVATE ONCE SENSOR WORKS
|
|
setSpinAngle();
|
|
}
|
|
|
|
void moveTo(int goalTo)
|
|
{
|
|
|
|
R1_Motor->moveTo(goalTo);
|
|
}
|
|
|
|
int distanceToGo()
|
|
{
|
|
return R1_Motor->distanceToGo();
|
|
}
|
|
|
|
//use this to determine true angle (over 360 and under 0) of the robot
|
|
int getTruAngle()
|
|
{
|
|
|
|
int currentAngle = posSensor.angleR(U_DEG,true)-R1_ANGLE_OFFSET;
|
|
if (currentAngle<0)
|
|
currentAngle = currentAngle+360;
|
|
|
|
// check if current angle is smaller than 180 but spin assumes greater angle
|
|
// then add 360 to result (and similar constelations)
|
|
|
|
// at init, spin is 0 - if the angle is smaller than 120, then turn spin to -1
|
|
// that way, the correct spin is set.
|
|
// initializing spin with -1 would mean that at a starting angle of over 240,
|
|
//the robot believes he is 'under' 0 and will turn counter clockwise instead of clockwise
|
|
if(currentAngle < 120 && (spin_R1 == -1 || spin_R1 == 0))
|
|
return currentAngle;
|
|
else if(currentAngle < 120 && spin_R1 == 1)
|
|
return currentAngle + 360;
|
|
else if(currentAngle > 240 && spin_R1 == -1)
|
|
return currentAngle - 360;
|
|
else if(currentAngle > 240 && spin_R1 == 1)
|
|
return currentAngle;
|
|
else if(currentAngle > 345 && spin_R1 == 0)
|
|
return currentAngle - 360;
|
|
else if(spin_R1 == 0)
|
|
return currentAngle;
|
|
else
|
|
return currentAngle;
|
|
}
|
|
//corrects the position of R1 according to sensor read
|
|
bool correction(double goal_R1)
|
|
{
|
|
while(getTruAngle() > goal_R1+1)
|
|
{
|
|
R1_Motor->move(-1);
|
|
while(R1_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
while(getTruAngle() < goal_R1-1)
|
|
{
|
|
R1_Motor->move(1);
|
|
while(R1_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
}
|
|
|
|
//use this if robot loses absolute position of R2
|
|
bool init_force_current()
|
|
{
|
|
//posSensor.setZeroReg();
|
|
R1_Motor->setCurrentPosition(0);
|
|
}
|
|
|
|
//initializes robot R2 to position 0 and sets it
|
|
bool init()
|
|
{
|
|
Serial.print("in R1 calib: ");
|
|
while(getTruAngle() > R1_ZERO_ANGLE+1 || getTruAngle() < R1_ZERO_ANGLE-1)
|
|
{
|
|
|
|
if(getTruAngle() > R1_ZERO_ANGLE+20)
|
|
{
|
|
R1_Motor->move(-20);
|
|
while(R1_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
else if(getTruAngle() < R1_ZERO_ANGLE-20)
|
|
{
|
|
R1_Motor->move(20);
|
|
while(R1_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
|
|
else if(getTruAngle() > R1_ZERO_ANGLE+1)
|
|
{
|
|
R1_Motor->move(-1);
|
|
while(R1_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
else if(getTruAngle() < R1_ZERO_ANGLE-1)
|
|
{
|
|
R1_Motor->move(1);
|
|
while(R1_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
|
|
}
|
|
correction(0);
|
|
R1_Motor->setCurrentPosition(0);
|
|
currentPose.R1 = R1_ZERO_ANGLE;
|
|
|
|
Serial.println("R1 initialized");
|
|
}
|
|
};
|
|
|
|
//class handler for P Motor
|
|
//partially extends the accelstepper library
|
|
//allows for init(),parameter change and constructor with (type, step, dir)
|
|
class P_Handler
|
|
{
|
|
private:
|
|
AccelStepper* P_Motor;
|
|
public:
|
|
P_Handler(int type, int step, int dir)
|
|
{
|
|
P_Motor = new AccelStepper (type,step,dir);
|
|
}
|
|
|
|
void parameter(float maxSpeed, float Speed, float Acceleration)
|
|
{
|
|
P_Motor->setMaxSpeed(maxSpeed); // Set Max Speed of Stepper (Slower to get better accuracy)
|
|
P_Motor->setSpeed(Speed);
|
|
P_Motor->setAcceleration(Acceleration); // Set Acceleration of Stepper
|
|
}
|
|
|
|
void setCurrentPosition(int curPos)
|
|
{
|
|
P_Motor->setCurrentPosition(curPos);
|
|
}
|
|
|
|
void run()
|
|
{
|
|
P_Motor->run();
|
|
}
|
|
|
|
void moveTo(int goalTo)
|
|
{
|
|
P_Motor->moveTo(goalTo);
|
|
}
|
|
|
|
int distanceToGo()
|
|
{
|
|
return P_Motor->distanceToGo();
|
|
}
|
|
void init()
|
|
{
|
|
if(endstop_reached()) //check if already at endstop
|
|
{
|
|
P_Motor->move(30);
|
|
while(P_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
while(!(endstop_reached())) //crude measurement
|
|
{
|
|
P_Motor->move(-10);
|
|
while(P_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
P_Motor->setCurrentPosition(0);
|
|
|
|
P_Motor->moveTo(50);
|
|
while(P_Motor->distanceToGo())
|
|
run();
|
|
|
|
while(!(endstop_reached())) //mid measurement
|
|
{
|
|
P_Motor->move(-5);
|
|
while(P_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
P_Motor->setCurrentPosition(0);
|
|
|
|
P_Motor->moveTo(10);
|
|
while(P_Motor->distanceToGo())
|
|
run();
|
|
|
|
while(!(endstop_reached())) //fine measurement
|
|
{
|
|
P_Motor->move(-1);
|
|
while(P_Motor->distanceToGo())
|
|
run();
|
|
}
|
|
P_Motor->setCurrentPosition(0);
|
|
Serial.println("initialized P");
|
|
currentPose.P = R2_HEIGHT_OFFSET;
|
|
}
|
|
};
|
|
|
|
class R2_Handler
|
|
{
|
|
private:
|
|
Servo R2;
|
|
public:
|
|
R2_Handler(int pin)
|
|
{
|
|
R2.attach(pin);
|
|
}
|
|
bool moveTo(int degree)
|
|
{
|
|
if(degree > 0 && degree < 170)
|
|
{
|
|
R2.write(180-degree);
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
};
|
|
|
|
|
|
template <class T>
|
|
class deque
|
|
{
|
|
|
|
private:
|
|
T *q;
|
|
uint16_t front = 0;
|
|
uint16_t back = 0;
|
|
uint16_t size = 0;
|
|
public:
|
|
deque(int mysize = 16):size(mysize){
|
|
q = new T[size];
|
|
}
|
|
~deque() {free(q);}
|
|
|
|
bool isfull()
|
|
{
|
|
int check_back=back+1;
|
|
if(check_back>size-1)
|
|
check_back=0;
|
|
if(check_back == front)
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
bool isempty()
|
|
{
|
|
if(back == front)
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
bool push_front(T newT)
|
|
{
|
|
if(isfull())
|
|
return 0;
|
|
q[front] = newT;
|
|
front--;
|
|
if(front<0)
|
|
front = size-1;
|
|
return 1;
|
|
}
|
|
bool push_back(T newT)
|
|
{
|
|
if(isfull())
|
|
return 0;
|
|
q[back] = newT;
|
|
back++;
|
|
if(back >= size)
|
|
back=0;
|
|
return 1;
|
|
}
|
|
T pop_front()
|
|
{
|
|
if(isempty())
|
|
Serial.println("deque empty, cant pop front");
|
|
int tront = front++;
|
|
if(front >= size)
|
|
front=0;
|
|
|
|
return q[tront];
|
|
}
|
|
T pop_back()
|
|
{
|
|
if(isempty())
|
|
Serial.println("deque empty, cant pop back");
|
|
int tack = back--;
|
|
if(back<0)
|
|
back = size-1;
|
|
return q[tack];
|
|
}
|
|
};
|
|
// -------- Motor -------- //
|
|
#define R2pin 9
|
|
R1_Handler* R1;
|
|
P_Handler* P;
|
|
R2_Handler* R2;
|
|
|
|
|
|
|
|
SoftwareSerial BTSerial(2, 3); // RX, TX
|
|
|
|
// -------- Sensor -------- //
|
|
//unit consts
|
|
#define U_RAW 1
|
|
#define U_TRN 2
|
|
#define U_DEG 3
|
|
#define U_RAD 4
|
|
#define U_GRAD 5
|
|
#define U_MOA 6
|
|
#define U_SOA 7
|
|
#define U_MILNATO 8
|
|
#define U_MILSE 9
|
|
#define U_MILRU 10
|
|
|
|
int incomingByte = 0;
|
|
int fromComFlag = 0;
|
|
int LED = 13;
|
|
int ledState = 0;
|
|
float actPos = 0;
|
|
|
|
#define ENDSTOP_PIN 4
|
|
#define MAGNET_EEF 10
|
|
|
|
// -------- GLOBALS -------- //
|
|
|
|
//q
|
|
|
|
deque<command>* Command_q;
|
|
uint32_t subCommand_q [64] = {};
|
|
int bad_com = 0;
|
|
|
|
//movement
|
|
uint16_t speed = 20; //mm/s
|
|
|
|
//statemachine
|
|
int state = 0;
|
|
|
|
// -------- MAIN -------- //
|
|
void setup() {
|
|
initialization_communication();
|
|
initialization_pins();
|
|
initialization_drive();
|
|
Serial.println("Setup Done");
|
|
//present();
|
|
Send_Im_Alive();
|
|
|
|
|
|
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
int legal_code;
|
|
getCommunication(); //read from serials
|
|
//sendStatus();
|
|
switch(state)
|
|
{
|
|
//waiting for new command
|
|
case 0:
|
|
if(!(Command_q->isempty()))
|
|
{
|
|
state=1;
|
|
}
|
|
break;
|
|
//calculate next subcommand_q and check if legal
|
|
case 1:
|
|
generate_subcommand();
|
|
Send_Im_Alive();
|
|
//legal_code = checkPositionLegality();
|
|
//if(legal_code)
|
|
state=0;
|
|
//else
|
|
// state=400+legal_code;
|
|
break;
|
|
/*//execute next command in subcommand_q
|
|
case 2:
|
|
execute_subcommfand();
|
|
if(!subCommand_q[0])
|
|
command_done();
|
|
break;
|
|
|
|
//high priority command
|
|
case 3:
|
|
//insert subcommand routine into
|
|
break;
|
|
|
|
|
|
//illegal subcommand: out of range
|
|
case 401:
|
|
//send error message, delete subcommand_q
|
|
break;
|
|
// illegal subcommand: in blocked zone
|
|
case 402:
|
|
//send error message, delete subcommand_q
|
|
break;*/
|
|
}
|
|
}
|
|
|
|
|
|
// ------ INIT FUNCTIONS -------- //
|
|
bool initialization_pins()
|
|
{
|
|
//initialize all Arduino Pins here
|
|
|
|
|
|
// init steppers
|
|
R1 = new R1_Handler(1,7,8);
|
|
P = new P_Handler(1,5,6);
|
|
|
|
R1->setCurrentPosition(0);
|
|
R1->parameter(50,50,MAX_ACCEL_R1);
|
|
|
|
|
|
P->setCurrentPosition(0); // Set the current position as zero for now
|
|
P->parameter(200,50,MAX_ACCEL_P);
|
|
// init klick sensor
|
|
pinMode(ENDSTOP_PIN,INPUT_PULLUP);
|
|
|
|
//init servo
|
|
R2 = new R2_Handler(R2pin);
|
|
|
|
// init magnet eef
|
|
pinMode(MAGNET_EEF,OUTPUT);
|
|
|
|
// init magnet sensor
|
|
// ACTIVATE IF SENSOR WORKS
|
|
R1->posSensor.begin();
|
|
|
|
Serial.println("initialized PINS");
|
|
// init magnet
|
|
return 0;
|
|
}
|
|
|
|
bool endstop_reached()
|
|
{
|
|
if(digitalRead(ENDSTOP_PIN)==true)
|
|
{
|
|
return 0;
|
|
}
|
|
else
|
|
return 1;
|
|
}
|
|
|
|
bool initialization_drive()
|
|
{
|
|
R2->moveTo(90);
|
|
delay(1000);
|
|
currentPose.R2 = 90;
|
|
//init R2
|
|
|
|
//init R
|
|
//init P
|
|
P->init();
|
|
//init R1
|
|
R1->init();
|
|
|
|
|
|
Pose home;
|
|
home.R1 = 0;
|
|
home.P = 100;
|
|
home.R2 = 90;
|
|
home.eef = 0;
|
|
moveA(home);
|
|
|
|
|
|
|
|
|
|
//drive all motors to 0 position
|
|
|
|
Serial.println("initialized DRIVE");
|
|
return 0;
|
|
}
|
|
|
|
bool present()
|
|
{
|
|
KartesianCoordinates home;
|
|
home.y = -8;
|
|
home.x = -130;
|
|
home.z = 10;
|
|
moveJ(home);
|
|
home.y = -8;
|
|
home.x = -130;
|
|
home.z = 20;
|
|
moveJ(home);
|
|
}
|
|
|
|
bool initialization_communication()
|
|
{
|
|
Serial.begin(9600);
|
|
Command_q = new deque<command>(Q_SIZE);
|
|
//start communication with raspi over serial
|
|
|
|
//start software-serial for bluetooth
|
|
BTSerial.begin(9600);
|
|
//mySerial.println("Hello you!");
|
|
Serial.println("initialized COMS");
|
|
|
|
return 0;
|
|
}
|
|
|
|
// -------- COMMUNICATION FUNCTIONS --------//
|
|
|
|
bool sendStatus()
|
|
{
|
|
int currentAngle = R1->posSensor.angleR(U_DEG,true)-R1_ANGLE_OFFSET;
|
|
if (currentAngle < 0)
|
|
currentAngle = currentAngle+360;
|
|
Serial.println(currentAngle, DEC);
|
|
Serial.println(R1->posSensor.angleR(U_DEG,true), DEC);
|
|
if(endstop_reached())
|
|
Serial.println("endstop Triggered");
|
|
}
|
|
|
|
bool availableSerial()
|
|
{
|
|
if(BTSerial.available() || Serial.available())
|
|
{
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
byte getNextSerial()
|
|
{
|
|
long int timer = millis();
|
|
while (timer+500>millis())
|
|
{
|
|
if(BTSerial.available())
|
|
return BTSerial.read();
|
|
|
|
if(Serial.available())
|
|
return Serial.read();
|
|
}
|
|
Serial.println("delay to long, terminating");
|
|
bad_com = 1;
|
|
return EOT1;
|
|
}
|
|
//remove next command from Q
|
|
void cleanQ()
|
|
{
|
|
Serial.println("cleaning until EOT");
|
|
byte nextData;
|
|
byte prevData = 0b00000000;
|
|
nextData = getNextSerial();
|
|
while(availableSerial())
|
|
{
|
|
Serial.println(nextData,BIN);
|
|
if(nextData == EOT2)
|
|
if(prevData == EOT1)
|
|
break;
|
|
prevData = nextData;
|
|
nextData = getNextSerial();
|
|
}
|
|
}
|
|
bool Send_Im_Alive()
|
|
{
|
|
//send signature ('A')
|
|
Serial.print('A');
|
|
Serial.print(currentPose.R1);
|
|
Serial.print(currentPose.P);
|
|
Serial.print(currentPose.R2);
|
|
Serial.print(currentPose.eef);
|
|
|
|
//send same to bluetooth
|
|
BTSerial.write(5);
|
|
|
|
|
|
Pose tempPose = Robot_to_SI_grad(currentPose);
|
|
byte low1 = tempPose.R1;
|
|
byte high1 = tempPose.R1 >> 8;
|
|
byte low2 = tempPose.P;
|
|
byte high2 = tempPose.P >> 8;
|
|
byte low3 = tempPose.R2;
|
|
byte high3 = tempPose.R2 >> 8;
|
|
|
|
char Str[9] = {5, high1, low1, high2, low2, high3, low3, EOT1, EOT2};
|
|
BTSerial.print(Str);
|
|
|
|
//BTSerial.print(tempPose.P);
|
|
//BTSerial.print(tempPose.R2);
|
|
//BTSerial.print(tempPose.eef);
|
|
|
|
|
|
}
|
|
|
|
long int comwait = 0;
|
|
|
|
//pulls data from available serial devices and puts them into q accordingly
|
|
bool getCommunication()
|
|
{
|
|
if(!availableSerial())
|
|
{
|
|
if(comwait + 2000 < millis())
|
|
{
|
|
//Serial.println("waiting...");
|
|
comwait = millis();
|
|
}
|
|
return 0;
|
|
}
|
|
if(Command_q->isfull())
|
|
{
|
|
Serial.println("command q full!");
|
|
}
|
|
while(availableSerial() && !(Command_q->isfull()))
|
|
{
|
|
|
|
int count = 0;
|
|
command newCommand;
|
|
|
|
//get Next Signature, if not valid, destroy command
|
|
newCommand.signature = getNextSerial();
|
|
//Serial.print("signature:");
|
|
//Serial.println(newCommand.signature,BIN);
|
|
if(!(newCommand.signature == 'R' || newCommand.signature == 'B'))
|
|
{
|
|
cleanQ();
|
|
Serial.println("signature invalid");
|
|
return 0;
|
|
}
|
|
//get Priority
|
|
newCommand.priority = getNextSerial();
|
|
//Serial.print("priority:");
|
|
//Serial.println(newCommand.priority,BIN);
|
|
//get type
|
|
newCommand.type = getNextSerial();
|
|
Serial.print("type:");
|
|
Serial.println(newCommand.type,BIN);
|
|
|
|
int datacount = 0;
|
|
//get entire data
|
|
byte nextData = getNextSerial();
|
|
Serial.println(nextData,BIN);
|
|
|
|
//depending on type, get the right number of data packages from the file stream
|
|
|
|
while(datacount < DATA_SIZE_IN_COMMAND-2)
|
|
{
|
|
if(nextData == EOT2)
|
|
if(newCommand.data[datacount-1] == EOT1)
|
|
break;
|
|
|
|
newCommand.data[datacount++] = nextData;
|
|
nextData = getNextSerial();
|
|
Serial.println(nextData,BIN);
|
|
}
|
|
if(bad_com)
|
|
{
|
|
Serial.println("Error, unfinished data package.");
|
|
bad_com = 0;
|
|
return 1;
|
|
}
|
|
//Serial.println("eot deteced");
|
|
newCommand.data[datacount] = EOT1; //adds terminating char to th end
|
|
|
|
//put it into the q if possible
|
|
if(Command_q->isfull())
|
|
Serial.println("Error: Command_q is Full");
|
|
else
|
|
if (newCommand.priority == 0b00000000)
|
|
Command_q->push_back(newCommand);
|
|
else if (newCommand.priority == 0b00000001)
|
|
Command_q->push_front(newCommand);
|
|
else if (newCommand.priority == 0b00000010)
|
|
Command_q->push_front(newCommand);
|
|
else
|
|
Serial.println("Error: invalid priority");
|
|
|
|
//Serial.println("Added to q");
|
|
}
|
|
return 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool generate_subcommand()
|
|
{
|
|
//read signature
|
|
command newCommand = Command_q->pop_front();
|
|
int16_t pausetime_ms;
|
|
switch(newCommand.type)
|
|
{
|
|
case 0b00000010: //MoveL command
|
|
case 0b0001000: // Kartesian moveJ command
|
|
KartesianCoordinates KC;
|
|
Serial.println("MoveJ KC");
|
|
KC.x = newCommand.data[0] * 256 + newCommand.data[1]; //converts the two bytes into an int16
|
|
KC.y = newCommand.data[2] * 256 + newCommand.data[3];
|
|
KC.z = newCommand.data[4] * 256 + newCommand.data[5];
|
|
if(newCommand.data[6] != EOT1) //check if there is a speed information
|
|
speed = newCommand.data[6] * 256 + newCommand.data[7];
|
|
return(moveJ(KC));
|
|
break;
|
|
case 0b00000001: //Polar moveJ command
|
|
PolarCoordinates PC;
|
|
Serial.println("MoveJ PC");
|
|
PC.t1 = newCommand.data[0] * 256 + newCommand.data[1]; //converts the two bytes into an int16
|
|
PC.r = newCommand.data[2] * 256 + newCommand.data[3];
|
|
PC.z = newCommand.data[4] * 256 + newCommand.data[5];
|
|
if(newCommand.data[6] != EOT1)
|
|
{
|
|
//check if there is a speed information
|
|
speed = newCommand.data[6] * 256 + newCommand.data[7];
|
|
}
|
|
return(moveJ(PC));
|
|
break;
|
|
case 0b00000100: //moveA commmand
|
|
Pose myPose;
|
|
Serial.println("MoveA");
|
|
myPose.R1 = newCommand.data[0] * 256 + newCommand.data[1]; //converts the two bytes into an int16
|
|
myPose.P = newCommand.data[2] * 256 + newCommand.data[3];
|
|
myPose.R2 = newCommand.data[4] * 256 + newCommand.data[5];
|
|
myPose.eef = newCommand.data[6]; // zero if off, one if on, else is dont change
|
|
moveA(myPose);
|
|
break;
|
|
case 0b00000111: //Pause command
|
|
Serial.println("Pause");
|
|
pausetime_ms = newCommand.data[0] * 256 + newCommand.data[1];
|
|
pause(pausetime_ms);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
//generates new subcommand from command
|
|
//switch case: movement J,C,L, type change, pause
|
|
return 0;
|
|
|
|
}
|
|
|
|
bool execute_subcommand()
|
|
{
|
|
//switch case: movement, type change, pause
|
|
return 0;
|
|
}
|
|
|
|
bool command_done()
|
|
{
|
|
//removes last command in command q and itterates remaining ones forward
|
|
return 0;
|
|
}
|
|
// -------- COORDNINATES AND MOVEMENTS --------- //
|
|
|
|
|
|
|
|
KartesianCoordinates xy_scew_correct(KartesianCoordinates myKC)
|
|
{
|
|
Serial.print("eliminating zy scew by: ");
|
|
double scew;
|
|
if(myKC.y>0)
|
|
{ if (ZY_SCEW_AT_P130)
|
|
scew = SCEW_RAD/ZY_SCEW_AT_P130;
|
|
else
|
|
scew = 0;
|
|
}
|
|
else
|
|
{ if (ZY_SCEW_AT_M130)
|
|
scew = SCEW_RAD/ZY_SCEW_AT_M130;
|
|
else
|
|
scew = 0;
|
|
}
|
|
KartesianCoordinates newKC;
|
|
newKC.x = myKC.x;
|
|
newKC.y = myKC.y;
|
|
double scew_add;
|
|
if(scew)
|
|
scew_add = abs(myKC.y)/scew;
|
|
else scew_add = 0;
|
|
Serial.println(scew_add);
|
|
//has to be minus, as z has to become higher, if the scew is positive as well
|
|
newKC.z = myKC.z + scew_add;
|
|
|
|
Serial.print("eliminating zx scew by: ");
|
|
|
|
if(myKC.x>0)
|
|
{ if (ZX_SCEW_AT_P130)
|
|
scew = SCEW_RAD/ZX_SCEW_AT_P130;
|
|
else
|
|
scew = 0;
|
|
}
|
|
else
|
|
{ if (ZX_SCEW_AT_M130)
|
|
scew = SCEW_RAD/ZX_SCEW_AT_M130;
|
|
else
|
|
scew = 0;
|
|
}
|
|
|
|
if(scew) scew_add = abs(myKC.x)/scew;
|
|
else scew_add = 0;
|
|
Serial.println(scew_add);
|
|
//has to be minus, as z has to become higher, if the scew is positive as well
|
|
newKC.z = newKC.z + scew_add;
|
|
return newKC;
|
|
}
|
|
|
|
//generates the inverse based on cartesian coordinates and the geometry of the robot
|
|
//pref is the preference, if the robot should access points from top or from bottom if possible
|
|
Pose inverse(KartesianCoordinates myKartesianCoordinates, int pref=0)
|
|
{
|
|
|
|
//correcting z position based on x,y position
|
|
myKartesianCoordinates = xy_scew_correct(myKartesianCoordinates);
|
|
double x = myKartesianCoordinates.x;
|
|
double y = myKartesianCoordinates.y;
|
|
double z = myKartesianCoordinates.z;
|
|
|
|
//Generate Errorpose
|
|
Pose errorPose;
|
|
errorPose.R1=-1;errorPose.P=-1;errorPose.R2=-1;
|
|
Pose myPose;
|
|
|
|
//returns angle between -180 and 180, adds y offset of R2
|
|
|
|
|
|
myPose.R1 = rad2grad(atan2(y, x));
|
|
//remap to 0 to 360
|
|
if(myPose.R1 < 0)
|
|
myPose.R1 = 360 + myPose.R1;
|
|
//calculate radius of R2 without default R2 offset
|
|
|
|
double l=sqrt(x*x+y*y)-R2_CENTER_OFFSET_X;
|
|
if(!pref)//dynamic from bot if possible
|
|
{
|
|
if(P_MAX_LOWER<(z+cos(asin(l/R2_LENGTH))*R2_LENGTH))
|
|
pref=2;
|
|
else
|
|
pref=1;
|
|
}
|
|
if(pref==1)//from top
|
|
{
|
|
// if real position of p beweglich greater max height or
|
|
// if real position of p beweglich smaller min height
|
|
// if l is longer than maximum length of r2
|
|
if((P_MAX_UP<(z+cos(asin(l/R2_LENGTH))*R2_LENGTH || R2_HEIGHT_OFFSET>(z+cos(asin(l/R2_LENGTH))*R2_LENGTH )) || l>R2_LENGTH))
|
|
{
|
|
Serial.println("Error, Pose not reachable! - TOP");
|
|
return errorPose; //error
|
|
}
|
|
myPose.R2 = rad2grad(asin(l/R2_LENGTH));
|
|
myPose.P = z+R2_LENGTH*cos((asin(l/R2_LENGTH)));
|
|
}
|
|
if(pref==2)//from bottom
|
|
{
|
|
if((P_MAX_UP<(z-cos(asin(l/R2_LENGTH))*R2_LENGTH)|| R2_HEIGHT_OFFSET>(z-cos(asin(l/R2_LENGTH))*R2_LENGTH)) || l>R2_LENGTH)
|
|
{
|
|
Serial.println("Error, Pose not reachable! - BOT");
|
|
return errorPose;
|
|
}
|
|
myPose.R2 = 180 - rad2grad(asin(l/R2_LENGTH));
|
|
myPose.P = z-R2_LENGTH*cos(asin(l/R2_LENGTH));
|
|
}
|
|
Serial.print("P: ");
|
|
Serial.println(myPose.P);
|
|
Serial.print("R2: ");
|
|
Serial.println(myPose.R2);
|
|
return myPose;
|
|
}
|
|
|
|
Pose inverse_full(KartesianCoordinates myKartesianCoordinates, int pref=0)
|
|
{
|
|
double x = myKartesianCoordinates.x;
|
|
double y = myKartesianCoordinates.y;
|
|
double z = myKartesianCoordinates.z;
|
|
//Generate Errorpose
|
|
Pose errorPose;
|
|
errorPose.R1=-1;errorPose.P=-1;errorPose.R2=-1;
|
|
Pose myPose;
|
|
|
|
//returns angle between -180 and 180, adds y offset of R2
|
|
|
|
|
|
myPose.R1 = rad2grad(atan2(y, x-R2_CENTER_OFFSET_Y/(cos(atan2(y,x)))));
|
|
//remap to 0 to 360
|
|
if(myPose.R1 < 0)
|
|
myPose.R1 = 360 + myPose.R1;
|
|
//calculate radius of R2 without default R2 offset
|
|
|
|
double l=sqrt(x*x+y*y-R2_CENTER_OFFSET_Y*R2_CENTER_OFFSET_Y)-R2_CENTER_OFFSET_X;
|
|
if(!pref)//dynamic from bot if possible
|
|
{
|
|
if(P_MAX_LOWER<(z+cos(asin(l/R2_LENGTH))*R2_LENGTH))
|
|
pref=2;
|
|
else
|
|
pref=1;
|
|
}
|
|
if(pref==1)//from top
|
|
{
|
|
// if real position of p beweglich greater max height or
|
|
// if real position of p beweglich smaller min height
|
|
// if l is longer than maximum length of r2
|
|
if((P_MAX_UP<(z+cos(asin(l/R2_LENGTH))*R2_LENGTH || R2_HEIGHT_OFFSET>(z+cos(asin(l/R2_LENGTH))*R2_LENGTH )) || l>R2_LENGTH))
|
|
{
|
|
Serial.println("Error, Pose not reachable! - TOP");
|
|
return errorPose; //error
|
|
}
|
|
myPose.R2 = rad2grad(asin(l/R2_LENGTH));
|
|
myPose.P = z+R2_LENGTH*cos((asin(l/R2_LENGTH)));
|
|
}
|
|
if(pref==2)//from bottom
|
|
{
|
|
if((P_MAX_UP<(z-cos(asin(l/R2_LENGTH))*R2_LENGTH)|| R2_HEIGHT_OFFSET>(z-cos(asin(l/R2_LENGTH))*R2_LENGTH)) || l>R2_LENGTH)
|
|
{
|
|
Serial.println("Error, Pose not reachable! - BOT");
|
|
return errorPose;
|
|
}
|
|
myPose.R2 = 180 - rad2grad(asin(l/R2_LENGTH));
|
|
myPose.P = z-R2_LENGTH*cos(asin(l/R2_LENGTH));
|
|
}
|
|
return myPose;
|
|
}
|
|
|
|
Pose inverse(PolarCoordinates myPolarCoordinates, int pref=1)
|
|
{
|
|
double t1 = myPolarCoordinates.t1;
|
|
double r = myPolarCoordinates.r;
|
|
double z = myPolarCoordinates.z;
|
|
|
|
KartesianCoordinates myKartesianCoordinates = transform(myPolarCoordinates);
|
|
|
|
return inverse(myKartesianCoordinates, pref);
|
|
}
|
|
//calculates from mm to rotations etc.
|
|
|
|
Pose SI_to_Robot(Pose SI_Pose) //muss noch editiert werden je nachdem wie der Roboter reagiert
|
|
{
|
|
Pose Robot_Pose;
|
|
Robot_Pose.R1 = rad2grad(SI_Pose.R1)/0.9; //number of steps from 0
|
|
Robot_Pose.P = SI_Pose.P*P_STEP_MULTIPLY - R2_HEIGHT_OFFSET*P_STEP_MULTIPLY; //number of steps from 0
|
|
Robot_Pose.R2 = rad2grad(SI_Pose.R2);
|
|
Robot_Pose.eef = SI_Pose.eef;
|
|
return Robot_Pose;
|
|
}
|
|
|
|
Pose SI_grad_to_Robot(Pose SI_Pose) //muss noch editiert werden je nachdem wie der Roboter reagiert
|
|
{
|
|
Pose Robot_Pose;
|
|
Robot_Pose.R1 = SI_Pose.R1/0.9; //number of steps from 0
|
|
double before = SI_Pose.P*P_STEP_MULTIPLY;
|
|
double without = R2_HEIGHT_OFFSET*P_STEP_MULTIPLY;
|
|
Robot_Pose.P = before - without; //number of steps from 0
|
|
Robot_Pose.R2 = SI_Pose.R2;
|
|
Robot_Pose.eef = SI_Pose.eef;
|
|
return Robot_Pose;
|
|
}
|
|
|
|
Pose Robot_to_SI_grad(Pose Robot_Pose)
|
|
{
|
|
Pose SI_Pose;
|
|
SI_Pose.R1 = Robot_Pose.R1*0.9; //number of steps from 0
|
|
SI_Pose.P = Robot_Pose.P/P_STEP_MULTIPLY + R2_HEIGHT_OFFSET;
|
|
SI_Pose.R2 = Robot_Pose.R2;
|
|
SI_Pose.eef = Robot_Pose.eef;
|
|
return SI_Pose;
|
|
}
|
|
|
|
double rad2grad(double rad)
|
|
{
|
|
return rad*180/3.141;
|
|
}
|
|
|
|
KartesianCoordinates transform(PolarCoordinates oldPolarCoordinates)
|
|
{ //transforms polar with degree to kartesian
|
|
KartesianCoordinates newKartesianCoordinates;
|
|
newKartesianCoordinates.x=oldPolarCoordinates.r*cos(oldPolarCoordinates.t1*3.141/180);
|
|
newKartesianCoordinates.y=oldPolarCoordinates.r*sin(oldPolarCoordinates.t1*3.141/180);
|
|
newKartesianCoordinates.z=oldPolarCoordinates.z;
|
|
|
|
return newKartesianCoordinates;
|
|
}
|
|
|
|
PolarCoordinates transform(KartesianCoordinates oKC)
|
|
{ //transforms kartesian to polar
|
|
PolarCoordinates newPolarCoordinates;
|
|
newPolarCoordinates.r=sqrt(oKC.x*oKC.x+oKC.y*oKC.y);
|
|
newPolarCoordinates.t1=atan2(oKC.y,oKC.x);
|
|
newPolarCoordinates.z=oKC.z;
|
|
|
|
return newPolarCoordinates;
|
|
}
|
|
|
|
bool checkPositionLegality(Pose myPose)
|
|
{
|
|
bool illegal = 0;
|
|
|
|
//check if out of range and not the 'dont change' parameter
|
|
if((myPose.R1 < 0 || myPose.R1 > 359) && myPose.R1 != DONT_CHANGE)
|
|
{
|
|
Serial.println("Sorry, R1 can't go there");
|
|
Serial.print("R1 = ");
|
|
Serial.println(myPose.R1);
|
|
|
|
illegal = 1;
|
|
}
|
|
if((myPose.P < R2_HEIGHT_OFFSET || myPose.P > P_MAX_UP) && myPose.P != DONT_CHANGE)
|
|
{
|
|
Serial.println("Sorry, P can't go there:");
|
|
Serial.print("P = ");
|
|
Serial.println(myPose.P);
|
|
illegal = 1;
|
|
}
|
|
if((myPose.R2 < 0 || myPose.R2 > 180) && myPose.R2 != DONT_CHANGE)
|
|
{
|
|
Serial.println("Sorry, R2 can't go there");
|
|
Serial.print("R2 = ");
|
|
Serial.println(myPose.R2);
|
|
illegal = 1;
|
|
}
|
|
|
|
if(illegal)
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
bool pause(int16_t pause_ms)
|
|
{
|
|
delay(pause_ms);
|
|
}
|
|
|
|
bool moveL()
|
|
{
|
|
//generate subcommands for move L
|
|
return 0;
|
|
}
|
|
|
|
bool moveC()
|
|
{
|
|
//generate subcommands for move C
|
|
return 0;
|
|
}
|
|
|
|
bool moveJ(KartesianCoordinates KC)
|
|
{
|
|
|
|
Serial.println("Goalpose in Kartesian:");
|
|
Serial.println(KC.x);
|
|
Serial.println(KC.y);
|
|
Serial.println(KC.z);
|
|
Pose goalPose = inverse(KC);
|
|
/*
|
|
Serial.println("Goalpose in SI:");
|
|
Serial.println(goalPose.R1);
|
|
Serial.println(goalPose.P);
|
|
Serial.println(goalPose.R2);*/
|
|
|
|
if((goalPose.R1 == -1 && goalPose.P == -1) && goalPose.R2 == -1)
|
|
return 1;
|
|
|
|
if(checkPositionLegality(goalPose))
|
|
return 1;
|
|
//TODO! use unscaled R1, P to controll speed of stepper to match time of R2 movement (slower only, not faster)
|
|
goalPose=SI_grad_to_Robot(inverse(KC));
|
|
double unscaled_R1_time = abs(goalPose.R1 - currentPose.R1);
|
|
double unscaled_P_time = abs(goalPose.P - currentPose.P);
|
|
double unscaled_R2_time = abs(goalPose.R2 - currentPose.R2);
|
|
|
|
/*Serial.print("unscaled_R1_time: ");
|
|
Serial.println(unscaled_R1_time);
|
|
Serial.print("unscaled_P_time: ");
|
|
Serial.println(unscaled_P_time);*/
|
|
|
|
|
|
double Stepper_scaling;
|
|
double Servo_Seconds;
|
|
|
|
//regulate desired speed of steppers
|
|
//save speed gain os servo scaling
|
|
|
|
if(unscaled_P_time > unscaled_R1_time)
|
|
{
|
|
|
|
P ->parameter(speed*STEPPER_SPEED_SCALING,speed*STEPPER_SPEED_SCALING,MAX_ACCEL_P);//5 scales the speed to stepper speed
|
|
|
|
if(unscaled_P_time !=0)
|
|
Stepper_scaling = unscaled_R1_time/unscaled_P_time;
|
|
else
|
|
Stepper_scaling = 1;
|
|
|
|
if(Stepper_scaling < 0.25)
|
|
Stepper_scaling = 0.25;
|
|
//servo scaling is approx. time stepper will take
|
|
//servo always orients itself by P axis!
|
|
Servo_Seconds = 1000*(double)unscaled_P_time/(double)(speed*STEPPER_SPEED_SCALING);
|
|
R1->parameter(Stepper_scaling*speed*STEPPER_SPEED_SCALING,Stepper_scaling*speed*STEPPER_SPEED_SCALING,MAX_ACCEL_R1);
|
|
|
|
|
|
/*Serial.print("Speed P: ");
|
|
Serial.println(speed*STEPPER_SPEED_SCALING);
|
|
Serial.print("Speed R1: ");
|
|
Serial.println(Stepper_scaling*speed*STEPPER_SPEED_SCALING);*/
|
|
|
|
}
|
|
else
|
|
{
|
|
|
|
R1 ->parameter(speed*STEPPER_SPEED_SCALING,speed*STEPPER_SPEED_SCALING,MAX_ACCEL_R1);
|
|
|
|
if(unscaled_R1_time !=0)
|
|
Stepper_scaling = unscaled_P_time/unscaled_R1_time;
|
|
else
|
|
Stepper_scaling = 1;
|
|
|
|
if(Stepper_scaling < 0.25)
|
|
Stepper_scaling = 0.25;
|
|
|
|
//Servo always orients itself by P axis!
|
|
Servo_Seconds = 1000*(double)unscaled_P_time/(double)(speed*STEPPER_SPEED_SCALING);
|
|
P->parameter(Stepper_scaling*speed*STEPPER_SPEED_SCALING,Stepper_scaling*speed*STEPPER_SPEED_SCALING,MAX_ACCEL_P);
|
|
|
|
/*Serial.print("Speed P: ");
|
|
Serial.println(Stepper_scaling*speed*STEPPER_SPEED_SCALING);
|
|
Serial.print("Speed R1: ");
|
|
Serial.println(speed*STEPPER_SPEED_SCALING);*/
|
|
}
|
|
|
|
int R2_change = abs(goalPose.R2- currentPose.R2);
|
|
int R2_spin;
|
|
if(goalPose.R2-currentPose.R2 > 0)
|
|
R2_spin = 1;
|
|
else
|
|
R2_spin = -1;
|
|
|
|
double Servo_mspd;
|
|
long int servo_step_millis;
|
|
|
|
if(R2_change)
|
|
{
|
|
|
|
|
|
Servo_mspd = Servo_Seconds/R2_change;
|
|
servo_step_millis = millis();
|
|
|
|
//edit speed depending on accelerationtime of stepper
|
|
//currently hardcoded to 400 steps/s^2 for stepper
|
|
if(unscaled_P_time < 10)
|
|
Servo_mspd *= 16;
|
|
else if(unscaled_P_time < 20)
|
|
Servo_mspd *= 8;
|
|
else if(unscaled_P_time < 50)
|
|
Servo_mspd *=4;
|
|
else if(unscaled_P_time < 100)
|
|
Servo_mspd *=1.5;
|
|
else if(unscaled_P_time < 150)
|
|
Servo_mspd *=1.3;
|
|
//if not fluid, change to fluid and wait if goes down
|
|
if(Servo_mspd > 65)
|
|
{
|
|
//Serial.println("alterating");
|
|
//save time 'lost' through scaling down to 65 and change speed to 65
|
|
int R2_fluidity_adder = (Servo_mspd-65) * abs(R2_change);
|
|
Servo_mspd = 65;
|
|
|
|
//if servo movind downwards
|
|
if(R2_spin == -1)
|
|
{
|
|
//Serial.print("adding: ");
|
|
//Serial.println(R2_fluidity_adder);
|
|
servo_step_millis = servo_step_millis + R2_fluidity_adder;
|
|
}
|
|
}
|
|
}
|
|
//Serial.print("mspd: ");
|
|
//Serial.println(Servo_mspd);
|
|
R1->moveTo(goalPose.R1);
|
|
P->moveTo(goalPose.P);
|
|
|
|
while((P->distanceToGo() || R1->distanceToGo()) || R2_spin*currentPose.R2 < R2_spin*goalPose.R2)
|
|
{
|
|
//Serial.println(P->distanceToGo());
|
|
//Serial.println(R1->distanceToGo());
|
|
//Serial.println(R2_spin*currentPose.R2);
|
|
//Serial.println(R2_spin*goalPose.R2);
|
|
if(R2_change && ((servo_step_millis + Servo_mspd < millis()) && R2_spin*currentPose.R2 < R2_spin*goalPose.R2))
|
|
{
|
|
servo_step_millis = millis();
|
|
currentPose.R2 = currentPose.R2 + R2_spin;
|
|
R2->moveTo(currentPose.R2);
|
|
}
|
|
R1->run();
|
|
P->run();
|
|
|
|
|
|
}
|
|
R2->moveTo(goalPose.R2);
|
|
|
|
//ACTIVATE IF SENSOR WORKS
|
|
R1->correction(inverse(KC).R1);
|
|
|
|
//set goal pose to current pose at it will have been reached;
|
|
currentPose.R1 = goalPose.R1;
|
|
currentPose.R2 = goalPose.R2;
|
|
currentPose.P = goalPose.P;
|
|
return 0;
|
|
}
|
|
|
|
|
|
bool moveJ(PolarCoordinates PC)
|
|
{
|
|
Serial.println("Goalpose in Polar:");
|
|
Serial.println(PC.t1);
|
|
Serial.println(PC.r);
|
|
Serial.println(PC.z);
|
|
return moveJ(transform(PC));
|
|
}
|
|
|
|
|
|
bool moveA(Pose myPose)
|
|
{
|
|
Serial.println("Goalpose in SI:");
|
|
Serial.println(myPose.R1);
|
|
Serial.println(myPose.P);
|
|
Serial.println(myPose.R2);
|
|
Serial.println(myPose.eef);
|
|
//generate subcommands for move A
|
|
Pose goalPose = SI_grad_to_Robot(myPose);
|
|
|
|
//checks if desired position is a legal position -> returns if not
|
|
|
|
if(checkPositionLegality(myPose))
|
|
return 1;
|
|
if(myPose.R1 == DONT_CHANGE) //check if dont change, save current pose over goalpose
|
|
goalPose.R1 = currentPose.R1;
|
|
|
|
if(myPose.P == DONT_CHANGE)
|
|
goalPose.P = currentPose.P;
|
|
|
|
if(myPose.R2 == DONT_CHANGE)
|
|
goalPose.R2 = currentPose.R2;
|
|
|
|
if(myPose.eef == DONT_CHANGE)
|
|
goalPose.eef = currentPose.eef;
|
|
|
|
//calculating desired speed
|
|
double unscaled_R1_time = abs(goalPose.R1 - currentPose.R1);
|
|
double unscaled_P_time = abs(goalPose.P - currentPose.P);
|
|
double unscaled_R2_time = abs(goalPose.R2 - currentPose.R2);
|
|
|
|
double Stepper_scaling;
|
|
double Servo_Seconds;
|
|
|
|
//speedchange sollte in Funktionen verpackt werden!
|
|
//calculate desired speed of the steppers, calculate scale for servo from maximum
|
|
if(unscaled_P_time > unscaled_R1_time)
|
|
{
|
|
|
|
P ->parameter(speed*STEPPER_SPEED_SCALING,speed*STEPPER_SPEED_SCALING,MAX_ACCEL_P);//5 scales the speed to stepper speed
|
|
if(unscaled_P_time !=0)
|
|
Stepper_scaling = unscaled_R1_time/unscaled_P_time;
|
|
else
|
|
Stepper_scaling = 1;
|
|
|
|
if(Stepper_scaling < 0.25)
|
|
Stepper_scaling = 0.25;
|
|
//servo scaling is approx. time stepper will take
|
|
Servo_Seconds = 1000*(double)unscaled_P_time/(double)(speed*STEPPER_SPEED_SCALING);
|
|
R1->parameter(Stepper_scaling*speed*STEPPER_SPEED_SCALING,Stepper_scaling*speed*STEPPER_SPEED_SCALING,MAX_ACCEL_R1);
|
|
|
|
/*Serial.print("Speed P: ");
|
|
Serial.println(speed*STEPPER_SPEED_SCALING);
|
|
Serial.print("Speed R1: ");
|
|
Serial.println(Stepper_scaling*speed*STEPPER_SPEED_SCALING);*/
|
|
|
|
}
|
|
else
|
|
{
|
|
|
|
R1 ->parameter(speed*STEPPER_SPEED_SCALING,speed*STEPPER_SPEED_SCALING,MAX_ACCEL_R1);
|
|
|
|
if(unscaled_R1_time !=0)
|
|
Stepper_scaling = unscaled_P_time/unscaled_R1_time;
|
|
else
|
|
Stepper_scaling = 1;
|
|
|
|
if(Stepper_scaling < 0.25)
|
|
Stepper_scaling = 0.25;
|
|
|
|
Servo_Seconds = 1000*(double)unscaled_P_time/(double)(speed*STEPPER_SPEED_SCALING);
|
|
P->parameter(Stepper_scaling*speed*STEPPER_SPEED_SCALING,Stepper_scaling*speed*STEPPER_SPEED_SCALING,MAX_ACCEL_P);
|
|
|
|
/*Serial.print("Speed P: ");
|
|
Serial.println(Stepper_scaling*speed*STEPPER_SPEED_SCALING);
|
|
Serial.print("Speed R1: ");
|
|
Serial.println(speed*STEPPER_SPEED_SCALING);*/
|
|
}
|
|
|
|
int R2_change = abs(goalPose.R2- currentPose.R2);
|
|
int R2_spin;
|
|
if(goalPose.R2-currentPose.R2 > 0)
|
|
R2_spin = 1;
|
|
else
|
|
R2_spin = -1;
|
|
|
|
double Servo_mspd;
|
|
long int servo_step_millis;
|
|
if(R2_change)
|
|
{
|
|
Servo_mspd = Servo_Seconds/R2_change;
|
|
servo_step_millis = millis();
|
|
|
|
|
|
if(unscaled_P_time < 10)
|
|
Servo_mspd *= 16;
|
|
else if(unscaled_P_time < 20)
|
|
Servo_mspd *= 8;
|
|
else if(unscaled_P_time < 50)
|
|
Servo_mspd *=4;
|
|
else if(unscaled_P_time < 100)
|
|
Servo_mspd *=1.5;
|
|
else if(unscaled_P_time < 150)
|
|
Servo_mspd *=1.3;
|
|
|
|
//if not fluid, change to fluid and wait if goes down
|
|
if(Servo_mspd > 65)
|
|
{
|
|
//save time 'lost' through scaling down to 65 and change speed to 65
|
|
int R2_fluidity_adder = (Servo_mspd-65) * abs(R2_change);
|
|
Servo_mspd = 65;
|
|
|
|
//if servo movind downwards
|
|
if(R2_spin == -1)
|
|
{
|
|
//Serial.println("adjusting by:");
|
|
//Serial.println(R2_fluidity_adder);
|
|
servo_step_millis = servo_step_millis + R2_fluidity_adder;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
//move all axis to goal
|
|
R1->moveTo(goalPose.R1);
|
|
P->moveTo(goalPose.P);
|
|
|
|
while((P->distanceToGo() || R1->distanceToGo()) || R2_spin*currentPose.R2 < R2_spin*goalPose.R2)
|
|
{
|
|
if(R2_change && ((servo_step_millis + Servo_mspd < millis()) && R2_spin*currentPose.R2 < R2_spin*goalPose.R2))
|
|
{
|
|
//Serial.println("currentPose R2");
|
|
//Serial.println(currentPose.R2);
|
|
servo_step_millis = millis();
|
|
currentPose.R2 = currentPose.R2 + R2_spin;
|
|
R2->moveTo(currentPose.R2);
|
|
}
|
|
P->run();
|
|
R1->run();
|
|
}
|
|
//correcting R1 based on measurement
|
|
//back from steps to wanted angle
|
|
//ACTIVSTE IF SENSEOR WORKS
|
|
R2->moveTo(goalPose.R2);
|
|
R1->correction(goalPose.R1*0.9);
|
|
|
|
// set eef magnet
|
|
if(goalPose.eef == 1)
|
|
{
|
|
digitalWrite(MAGNET_EEF, HIGH);
|
|
}
|
|
else
|
|
{
|
|
digitalWrite(MAGNET_EEF,LOW);
|
|
}
|
|
//set goal pose to current pose at it will have been reached;
|
|
currentPose.R1 = goalPose.R1;
|
|
currentPose.R2 = goalPose.R2;
|
|
currentPose.P = goalPose.P;
|
|
currentPose.eef = goalPose.eef;
|
|
//check current position of R1
|
|
return 1;
|
|
} |