Commit 6c011672 authored by Gallacchi Mattia's avatar Gallacchi Mattia
Browse files

Add TMC5160 submodule 



Signed-off-by: default avatarmattia.gallacchi <mattia.gallacchi@he-arc.ch>
parent 5753a870

.gitmodules

0 → 100644
+3 −0
Original line number Diff line number Diff line 
[submodule "firmware/TMC5160"]

	path = firmware/TMC5160

	url = https://github.com/tommag/TMC5160_Arduino.git

TMC5160 @ 0471224c

Original line number Diff line number Diff line 
Subproject commit 0471224cc7e45d0ec6d3a89bacf4c7f79d845a5b

firmware/firmware.ino

0 → 100755
+231 −0
Original line number Diff line number Diff line 
#include <Arduino.h>

#include <TMC5160.h>



// axis 1

const uint8_t SPI_CS1 = 10; // CS pin in SPI mode

// axis 2

const uint8_t SPI_CS2 = 9; // CS pin in SPI mode

// axis 3

const uint8_t SPI_CS3 = 8; // CS pin in SPI mode



const uint8_t SPI_DRV_ENN = 22;  // DRV_ENN pin in SPI mode



TMC5160_SPI motor1 = TMC5160_SPI(SPI_CS1); // Use default SPI peripheral and SPI settings

TMC5160_SPI motor2 = TMC5160_SPI(SPI_CS2); // Use default SPI peripheral and SPI settings

TMC5160_SPI motor3 = TMC5160_SPI(SPI_CS3); // Use default SPI peripheral and SPI settings



float xactualold1 = 0.0;

float vactualold1 = 0.0;

float posa1 =0.0;

float xactualold2 = 0.0;

float vactualold2 = 0.0;

float posa2 =0.0;

float xactualold3 = 0.0;

float vactualold3 = 0.0;

float posa3 =0.0;

float xactualold4 = 0.0;

float vactualold4 = 0.0;

float posa4 =0.0;

float xactualold5 = 0.0;

float vactualold5 = 0.0;

float posa5 =0.0;





void setup() {

  // USB/debug serial coms

  Serial.begin(115200);

  delayMicroseconds(10);



  pinMode(SPI_DRV_ENN, OUTPUT); 

  digitalWrite(SPI_DRV_ENN, LOW); // Active low



  // This sets the motor & driver parameters /!\ run the configWizard for your driver and motor for fine tuning !

  TMC5160::PowerStageParameters powerStageParams1; // defaults.

  TMC5160::MotorParameters motorParams1;

  motorParams1.globalScaler = 100; // max 180 Adapt to your driver and motor (check TMC5160 datasheet - "Selecting sense resistors")

  motorParams1.irun = 31;

  motorParams1.ihold = 10;

  // This sets the motor & driver parameters /!\ run the configWizard for your driver and motor for fine tuning !

  TMC5160::PowerStageParameters powerStageParams2; // defaults.

  TMC5160::MotorParameters motorParams2;

  motorParams2.globalScaler = 100; // max 180 Adapt to your driver and motor (check TMC5160 datasheet - "Selecting sense resistors")

  motorParams2.irun = 31;

  motorParams2.ihold = 10;

  // This sets the motor & driver parameters /!\ run the configWizard for your driver and motor for fine tuning !

  TMC5160::PowerStageParameters powerStageParams3; // defaults.

  TMC5160::MotorParameters motorParams3;

  motorParams3.globalScaler = 100; // max 180 Adapt to your driver and motor (check TMC5160 datasheet - "Selecting sense resistors")

  motorParams3.irun = 31;

  motorParams3.ihold = 10;



  SPI.begin();

  motor1.begin(powerStageParams1, motorParams1, TMC5160::NORMAL_MOTOR_DIRECTION);

  motor2.begin(powerStageParams2, motorParams2, TMC5160::NORMAL_MOTOR_DIRECTION);

  motor3.begin(powerStageParams3, motorParams3, TMC5160::NORMAL_MOTOR_DIRECTION);

  

  // ramp definition

  motor1.setRampMode(TMC5160::POSITIONING_MODE);

  motor2.setRampMode(TMC5160::POSITIONING_MODE);

  motor3.setRampMode(TMC5160::POSITIONING_MODE);



  motor1.setMaxSpeed(30);

  motor1.setAcceleration(200);

  motor2.setMaxSpeed(30);

  motor2.setAcceleration(200);

  motor3.setMaxSpeed(30);

  motor3.setAcceleration(200);



  delay(1000); // Standstill for automatic tuning

  delay(100000); // Standstill to remove the Support Blocks



  //----------------------------------Move to the Home-----------------------



  //------------------------------End of Move to the Home--------------------

  Serial.println("starting up");

  int k=5;

  motor1.setMaxSpeed(15*k);

  motor1.setAcceleration(30*k);

  motor2.setMaxSpeed(15*k);

  motor2.setAcceleration(30*k);

  motor3.setMaxSpeed(15*k);

  motor3.setAcceleration(30*k);

  delay(1000);

}



void loop() {

  if (Serial.available()) {  // check for incoming serial data

    String RpiData = Serial.readStringUntil('\n');  // read command from serial port

    // get first target position

    String tempStr = RpiData.substring(1, 7);

    tempStr.trim();  // eliminate extra white spaces

    posa1 = tempStr.toFloat();

    tempStr = RpiData.substring(8, 14);

    tempStr.trim();  // eliminate extra white spaces

    posa2 = tempStr.toFloat();

    tempStr = RpiData.substring(15, 21);

    tempStr.trim();  // eliminate extra white spaces

    posa3 = tempStr.toFloat();

    tempStr = RpiData.substring(22, 28);

    tempStr.trim();  // eliminate extra white spaces

    posa4 = tempStr.toFloat();

    tempStr = RpiData.substring(29, 35);

    tempStr.trim();  // eliminate extra white spaces

    posa5 = tempStr.toFloat();

  }

  // check the limits

  posa1 = constrain(posa1, -50000.0, 50000.0);

  posa2 = constrain(posa2, -50000.0, 50000.0);

  posa3 = constrain(posa3, -50000.0, 50000.0);

  posa4 = constrain(posa4, -50000.0, 50000.0);

  posa5 = constrain(posa5, -50000.0, 50000.0);



  float theta_x = posa2/100.0/360.0*2.0*PI;

  float theta_y = posa3/100.0/360.0*2.0*PI;

  float theta_z = posa4/100.0/360.0*2.0*PI;



  // Inverse kinematics

  // First rotational matrix

  float Rot1M11 = cos(theta_x)+(1.0/3.0)*(1.0-cos(theta_x));

  float Rot1M12 = (1.0/3.0)*(1.0-cos(theta_x))+(1.0/sqrt(3.0))*sin(theta_x);

  float Rot1M13 = (1.0/3.0)*(1.0-cos(theta_x))-(1.0/sqrt(3.0))*sin(theta_x);

  float Rot1M21 = (1.0/3.0)*(1.0-cos(theta_x))-(1.0/sqrt(3.0))*sin(theta_x);

  float Rot1M22 = cos(theta_x)+(1.0/3.0)*(1.0-cos(theta_x));

  float Rot1M23 = (1.0/3.0)*(1.0-cos(theta_x))+(1.0/sqrt(3.0))*sin(theta_x);

  float Rot1M31 = (1.0/3.0)*(1.0-cos(theta_x))+(1.0/sqrt(3.0))*sin(theta_x);

  float Rot1M32 = (1.0/3.0)*(1.0-cos(theta_x))-(1.0/sqrt(3.0))*sin(theta_x);

  float Rot1M33 = cos(theta_x)+(1.0/3.0)*(1.0-cos(theta_x));

  // Second rotational matrix

  float Rot2M11 = cos(theta_y)+(1.0/2.0)*(1.0-cos(theta_y));

  float Rot2M12 = -(1.0/2.0)*(1.0-cos(theta_y));

  float Rot2M13 = (1.0/sqrt(2.0))*sin(theta_y);

  float Rot2M21 = -(1.0/2.0)*(1.0-cos(theta_y));

  float Rot2M22 = cos(theta_y)+(1.0/2.0)*(1.0-cos(theta_y));

  float Rot2M23 = (1.0/sqrt(2.0))*sin(theta_y);

  float Rot2M31 = -(1.0/sqrt(2.0))*sin(theta_y);

  float Rot2M32 = -(1.0/sqrt(2.0))*sin(theta_y);

  float Rot2M33 = cos(theta_y);

  // Third rotational matrix

  float Rot3M11 = cos(theta_z)+(1.0/3.0)*(1.0-cos(theta_z));

  float Rot3M12 = (1.0/3.0)*(1.0-cos(theta_z))+(1.0/sqrt(3.0))*sin(theta_z);

  float Rot3M13 = (1.0/3.0)*(1.0-cos(theta_z))-(1.0/sqrt(3.0))*sin(theta_z);

  float Rot3M21 = (1.0/3.0)*(1.0-cos(theta_z))-(1.0/sqrt(3.0))*sin(theta_z);

  float Rot3M22 = cos(theta_z)+(1.0/3.0)*(1.0-cos(theta_z));

  float Rot3M23 = (1.0/3.0)*(1.0-cos(theta_z))+(1.0/sqrt(3.0))*sin(theta_z);

  float Rot3M31 = (1.0/3.0)*(1.0-cos(theta_z))+(1.0/sqrt(3.0))*sin(theta_z);

  float Rot3M32 = (1.0/3.0)*(1.0-cos(theta_z))-(1.0/sqrt(3.0))*sin(theta_z);

  float Rot3M33 = cos(theta_z)+(1.0/3.0)*(1.0-cos(theta_z));

  // Second Rot Matrix x First Rot Matrix

  float Rot2xRot1M11 = Rot2M11*Rot1M11+Rot2M12*Rot1M21+Rot2M13*Rot1M31;

  float Rot2xRot1M12 = Rot2M11*Rot1M12+Rot2M12*Rot1M22+Rot2M13*Rot1M32;

  float Rot2xRot1M13 = Rot2M11*Rot1M13+Rot2M12*Rot1M23+Rot2M13*Rot1M33;

  float Rot2xRot1M21 = Rot2M21*Rot1M11+Rot2M22*Rot1M21+Rot2M23*Rot1M31;

  float Rot2xRot1M22 = Rot2M21*Rot1M12+Rot2M22*Rot1M22+Rot2M23*Rot1M32;

  float Rot2xRot1M23 = Rot2M21*Rot1M13+Rot2M22*Rot1M23+Rot2M23*Rot1M33;

  float Rot2xRot1M31 = Rot2M31*Rot1M11+Rot2M32*Rot1M21+Rot2M33*Rot1M31;

  float Rot2xRot1M32 = Rot2M31*Rot1M12+Rot2M32*Rot1M22+Rot2M33*Rot1M32;

  float Rot2xRot1M33 = Rot2M31*Rot1M13+Rot2M32*Rot1M23+Rot2M33*Rot1M33;

  // Third Rot Matrix x previous result

  float RotF11 = Rot3M11*Rot2xRot1M11+Rot3M12*Rot2xRot1M21+Rot3M13*Rot2xRot1M31;

  float RotF12 = Rot3M11*Rot2xRot1M12+Rot3M12*Rot2xRot1M22+Rot3M13*Rot2xRot1M32;

  float RotF13 = Rot3M11*Rot2xRot1M13+Rot3M12*Rot2xRot1M23+Rot3M13*Rot2xRot1M33;

  float RotF21 = Rot3M21*Rot2xRot1M11+Rot3M22*Rot2xRot1M21+Rot3M23*Rot2xRot1M31;

  float RotF22 = Rot3M21*Rot2xRot1M12+Rot3M22*Rot2xRot1M22+Rot3M23*Rot2xRot1M32;

  float RotF23 = Rot3M21*Rot2xRot1M13+Rot3M22*Rot2xRot1M23+Rot3M23*Rot2xRot1M33;

  float RotF31 = Rot3M31*Rot2xRot1M11+Rot3M32*Rot2xRot1M21+Rot3M33*Rot2xRot1M31;

  float RotF32 = Rot3M31*Rot2xRot1M12+Rot3M32*Rot2xRot1M22+Rot3M33*Rot2xRot1M32;

  float RotF33 = Rot3M31*Rot2xRot1M13+Rot3M32*Rot2xRot1M23+Rot3M33*Rot2xRot1M33;

  // Calculate motor angles in rad

  float theta1 = atan(RotF32/RotF22);

  float theta2 = atan(RotF21/RotF11);

  float theta3 = atan(RotF13/RotF33);

  

  // move axis

  motor1.setTargetPosition(108.0/20.0*(theta1-PI/4.0)*200.0/(2.0*PI));

  motor2.setTargetPosition(108.0/20.0*(theta2-PI/4.0)*200.0/(2.0*PI));

  motor3.setTargetPosition(108.0/20.0*(theta3-PI/4.0)*200.0/(2.0*PI));



  //send data back

  float xactual1 = 0.0;

  float xactual2 = posa2/10.0; //motor1.getCurrentPosition()*50.0;

  float xactual3 = posa3/10.0; //motor2.getCurrentPosition()*50.0;

  float xactual4 = posa4/10.0; //motor3.getCurrentPosition()*50.0;

  float xactual5 = 0.0;

  if isnan(xactual1) xactual1 = xactualold1;

  if isnan(xactual2) xactual2 = xactualold2;

  if isnan(xactual3) xactual3 = xactualold3;

  if isnan(xactual4) xactual4 = xactualold4;

  if isnan(xactual5) xactual5 = xactualold5;

  xactualold1 = xactual1;

  xactualold2 = xactual2;

  xactualold3 = xactual3;

  xactualold4 = xactual4;

  xactualold5 = xactual5;

  float vactual1 = 0.0;

  float vactual2 = motor1.getCurrentSpeed();

  float vactual3 = motor2.getCurrentSpeed();

  float vactual4 = motor3.getCurrentSpeed();

  float vactual5 = 0.0;

  if isnan(vactual1) vactual1 = vactualold1;

  if isnan(vactual2) vactual2 = vactualold2;

  if isnan(vactual3) vactual3 = vactualold3;

  if isnan(vactual4) vactual4 = vactualold4;

  if isnan(vactual5) vactual5 = vactualold5;

  vactualold1 = vactual1;

  vactualold2 = vactual2;

  vactualold3 = vactual3;

  vactualold4 = vactual4;

  vactualold5 = vactual5;

  Serial.print("a");

  Serial.print((xactual1+5000)/10000, 5);

  Serial.print("b");

  Serial.print((xactual2+5000)/10000, 5);

  Serial.print("c");

  Serial.print((xactual3+5000)/10000, 5);

  Serial.print("d");

  Serial.print((xactual4+5000)/10000, 5);

  Serial.print("e");

  Serial.print((xactual5+5000)/10000, 5);

  Serial.print("v");

  Serial.print((abs(vactual1)+abs(vactual2)+abs(vactual3)+abs(vactual4)+abs(vactual5))/(5*1000), 2);

  Serial.println("x");

}