//General #include #include #include //Motor //#include #include #include //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 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_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(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; }