RPR/02_Software/01_Arduino/main/main.ino

//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;
}