Add UI

build/

# python stuff

__pycache__/

 No newline at end of file

            "compilerArgs": [],

            "intelliSenseMode": "linux-clang-x64",

            "includePath": [

                "${workspaceFolder}/**"

                "${workspaceFolder}/**",

                "~/Arduino/libraries/**"

            ],

            "forcedInclude": [],

            "cStandard": "c17",

# Spherical Parallel Joint Robot

## Getting started

To make it easy for you to get started with GitLab, here's a list of recommended next steps.

Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)!

## Add your files

- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files

- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command:

```

cd existing_repo

git remote add origin https://labinfo.ing.he-arc.ch/gitlab/igib/public/spherical-parallel-joint-robot.git

git branch -M main

git push -uf origin main

```

## Integrate with your tools

- [ ] [Set up project integrations](https://labinfo.ing.he-arc.ch/gitlab/igib/public/spherical-parallel-joint-robot/-/settings/integrations)

## Collaborate with your team

- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/)

- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html)

- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically)

- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/)

- [ ] [Set auto-merge](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html)

## Test and Deploy

Use the built-in continuous integration in GitLab.

- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html)

- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing (SAST)](https://docs.gitlab.com/ee/user/application_security/sast/)

- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html)

- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/)

- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html)

***

# Editing this README

When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thanks to [makeareadme.com](https://www.makeareadme.com/) for this template.

## Suggestions for a good README

Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information.

## Name

Choose a self-explaining name for your project.

## Description

Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors.

## Badges

On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge.

## Visuals

Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method.

## Installation

Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection.

## Usage

Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README.

## Support

Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc.

## Roadmap

If you have ideas for releases in the future, it is a good idea to list them in the README.

## Contributing

State if you are open to contributions and what your requirements are for accepting them.

For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self.

You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser.

## Authors and acknowledgment

Show your appreciation to those who have contributed to the project.

## License

For open source projects, say how it is licensed.

## Project status

If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers.

# Teensy firmware

## Dependencies

Either clone the libraries under *~/Arduino/libraries/* or use Arduino library manager to install them.

| Library | Version | URL |

|---------|---------|-----|

| TMC5160_Arduino | v1.1.0 | https://github.com/tommag/TMC5160_Arduino/tree/v1.1.0 |

| ArxTypeTraits | v0.3.1 | https://github.com/hideakitai/ArxTypeTraits.git |

#include <Arduino.h>

#include <TMC5160.h>

#define DBG Serial1

enum ROTATION_AXIS{

    RZ = 0,

    RY,

    RZ1

};

// axis 1

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

// axis 2

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() {

void setup()

{

	// USB/debug serial coms

	Serial.begin(115200);

	DBG.begin(115200);

	delayMicroseconds(10);

	pinMode(SPI_DRV_ENN, OUTPUT);

	motor3.setAcceleration(200);

	delay(1000); // Standstill for automatic tuning

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

    // Wait acknowledge

    while(!Serial.available()) {

		DBG.println("Remove standstill");

		delay(250);

	}

	Serial.clear();

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

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

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

  Serial.println("starting up");

	DBG.println("starting up");

	int k = 5;

	motor1.setMaxSpeed(15 * k);

	motor1.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();

bool parse_data(String data, float* values)

{

	bool ret = true;

	int count = 0;

	// data.indexOf()

	while (count < 3) {

		int index = data.indexOf(",");

		values[count++] = data.substring(0, index).toFloat();

		data = data.substring(index + 1);

	}

	return ret;

}

  // 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");

void loop()

{

	float new_pos[3] = {0.0};

	if (Serial.available()) {

		String data = Serial.readStringUntil('\n');

		if (!parse_data(data, new_pos)) {

			goto send_current_pos;

		}

		DBG.printf("New pos: %f, %f, %f", new_pos[ROTATION_AXIS::RZ], new_pos[ROTATION_AXIS::RY], new_pos[ROTATION_AXIS::RZ1]);

		DBG.println();

		motor1.setTargetPosition(new_pos[ROTATION_AXIS::RZ]);

		motor2.setTargetPosition(new_pos[ROTATION_AXIS::RY]);

		motor3.setTargetPosition(new_pos[ROTATION_AXIS::RZ1]);

	}

send_current_pos:

	float current_pos[3] = {

		motor1.getCurrentPosition(),

		motor2.getCurrentPosition(),

		motor3.getCurrentPosition()

	};

	// DBG.printf("Current pos: %f, %f, %f", current_pos[ROTATION_AXIS::RZ], current_pos[ROTATION_AXIS::RY], current_pos[ROTATION_AXIS::RZ1]);

	// DBG.println();

	Serial.printf("%f,%f,%f", current_pos[ROTATION_AXIS::RZ], current_pos[ROTATION_AXIS::RY], current_pos[ROTATION_AXIS::RZ1]);

	Serial.println();

	delay(10);

}