|
@@ -0,0 +1,480 @@
|
|
|
+#include <AS5600.h>
|
|
|
+#include "AX12A.h"
|
|
|
+
|
|
|
+#define DirectionPin (14u)
|
|
|
+#define BaudRate (1000000ul)
|
|
|
+#define ID1 (4u)
|
|
|
+#define ID2 (5u)
|
|
|
+#include <SoftwareSerial.h>
|
|
|
+#include <XYZrobotServo.h>
|
|
|
+
|
|
|
+
|
|
|
+//AS5600 Sensor
|
|
|
+#include <Wire.h>
|
|
|
+
|
|
|
+SoftwareSerial servoSerial(2, 4); //(18,19)
|
|
|
+
|
|
|
+
|
|
|
+int input = 0;
|
|
|
+XYZrobotServo servo1(servoSerial, 2);
|
|
|
+
|
|
|
+const uint8_t playtime = 100;
|
|
|
+const int max_angle = 712,min_angle = 312;
|
|
|
+
|
|
|
+const byte numChars = 32;
|
|
|
+int dir = 25; //16
|
|
|
+//int brake = 5;
|
|
|
+int pwma = 27;
|
|
|
+int speed = 0;
|
|
|
+int i;
|
|
|
+//int Turns = 7;
|
|
|
+//bool State = 0;
|
|
|
+volatile bool Startup = 0;
|
|
|
+volatile bool richtung;
|
|
|
+const byte Test_button = 14;
|
|
|
+const byte button_oben = 13; //25
|
|
|
+const byte button_unten = 26;
|
|
|
+int magnetStatus = 0; //value of the status register (MD, ML, MH)
|
|
|
+
|
|
|
+int lowbyte; //raw angle 7:0
|
|
|
+word highbyte; //raw angle 7:0 and 11:8
|
|
|
+int rawAngle; //final raw angle
|
|
|
+float degAngle; //raw angle in degrees (360/4096 * [value between 0-4095])
|
|
|
+
|
|
|
+int quadrantNumber, previousquadrantNumber; //quadrant IDs
|
|
|
+int numberofTurns = 0; //number of turns
|
|
|
+int lastnumberofTurns = 0;
|
|
|
+float correctedAngle = 0; //tared angle - based on the startup value
|
|
|
+float startAngle = 0; //starting angle
|
|
|
+float totalAngle = 0; //total absolute angular displacement
|
|
|
+float previoustotalAngle = 0; //for the display printing
|
|
|
+
|
|
|
+int sollposition; //von 0 bis 1 * totalTurns
|
|
|
+
|
|
|
+ // initialize prozent to an invalid value
|
|
|
+int previousProzent = -1; // variable to store the previous valid value
|
|
|
+int prozent;
|
|
|
+int totalTurns; // number of Turns after Kalibration sollte ungefähr 33 entsprechen (23/0.7)
|
|
|
+
|
|
|
+int x ,position;
|
|
|
+int val[10] = {0,0,0,0,0};
|
|
|
+int choice;
|
|
|
+
|
|
|
+
|
|
|
+void IRAM_ATTR nachUnten(){
|
|
|
+ richtung = HIGH;
|
|
|
+ speed = 255;
|
|
|
+ //sollposition = sollposition - 3;
|
|
|
+}
|
|
|
+void IRAM_ATTR nachOben(){
|
|
|
+ richtung = LOW;
|
|
|
+ speed = 255;
|
|
|
+ //sollposition = sollposition + 3;
|
|
|
+}
|
|
|
+void IRAM_ATTR Start(){
|
|
|
+ Startup = 1;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void setup() {
|
|
|
+ Serial.begin(115200); //start serial - tip: don't use serial if you don't need it (speed considerations)
|
|
|
+ Wire.begin();
|
|
|
+
|
|
|
+ ax12a.begin(BaudRate, DirectionPin, &Serial2);
|
|
|
+ ax12a.setEndless(ID1, OFF);
|
|
|
+ ax12a.setEndless(ID2, OFF);
|
|
|
+ ax12a.setMaxTorque(ID1, 1000);
|
|
|
+ ax12a.setMaxTorque(ID2, 1000);
|
|
|
+
|
|
|
+ servoSerial.begin(115200);
|
|
|
+
|
|
|
+ ax12a.move(ID1,512);
|
|
|
+ ax12a.move(ID2,512);
|
|
|
+ servo1.setPosition(512, playtime);
|
|
|
+
|
|
|
+// pinMode(Turns, OUTPUT);
|
|
|
+ pinMode(dir, OUTPUT);
|
|
|
+// pinMode(brake, OUTPUT);
|
|
|
+ pinMode(pwma, OUTPUT);
|
|
|
+ pinMode(button_oben, INPUT_PULLUP);
|
|
|
+ pinMode(button_unten, INPUT_PULLUP);
|
|
|
+ pinMode(Test_button, INPUT_PULLUP);
|
|
|
+
|
|
|
+ //Kalibrieren();
|
|
|
+
|
|
|
+ attachInterrupt(digitalPinToInterrupt(button_oben), nachUnten, FALLING);
|
|
|
+ attachInterrupt(digitalPinToInterrupt(button_unten), nachOben, FALLING);
|
|
|
+ attachInterrupt(digitalPinToInterrupt(Test_button), Start, FALLING);
|
|
|
+
|
|
|
+ //start i2C
|
|
|
+ //Wire.setClock(800000L); //fast clock
|
|
|
+
|
|
|
+checkMagnetPresence(); //check the magnet (blocks until magnet is found)
|
|
|
+
|
|
|
+ ReadRawAngle(); //make a reading so the degAngle gets updated
|
|
|
+ startAngle = degAngle; //update startAngle with degAngle - for taring
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+/*void motor(int prozent) {
|
|
|
+
|
|
|
+ sollposition = (totalTurns * prozent * 0.01);
|
|
|
+ Serial.print("sollposition: ");
|
|
|
+ Serial.println(sollposition);
|
|
|
+ if (sollposition > numberofTurns) {
|
|
|
+ richtung = LOW;
|
|
|
+ speed = 255;
|
|
|
+ //Serial.println("Bewegung nach Oben");
|
|
|
+ } else if (sollposition < numberofTurns) {
|
|
|
+ richtung = HIGH;
|
|
|
+ speed = 255;
|
|
|
+ //Serial.println("Bewegung nach Unten");
|
|
|
+ } else {
|
|
|
+ speed = 0;
|
|
|
+ Serial.println("In Sollposition");
|
|
|
+ }
|
|
|
+ }*/
|
|
|
+ /*void Positionsabfrage() {
|
|
|
+ if (prozent == -1 && Serial.available() > 0) { // check if prozent is not set and data is available to read
|
|
|
+ int newProzent = Serial.parseInt(); // read the integer value from serial buffer
|
|
|
+
|
|
|
+ if (newProzent >= 1 && newProzent <= 100) { // check if the value is within the valid range
|
|
|
+ prozent = newProzent; // assign the new valid value to prozent
|
|
|
+ previousProzent = prozent; // update previousProzent with the new valid value
|
|
|
+ Serial.print("Received percentage value: ");
|
|
|
+ Serial.println(prozent);
|
|
|
+ } else {
|
|
|
+ Serial.println("Invalid percentage value! Please enter a number between 0 and 100.");
|
|
|
+ if (previousProzent != -1) { // check if there's a previous valid value
|
|
|
+ prozent = previousProzent; // restore prozent to its previous valid value
|
|
|
+ }
|
|
|
+ }
|
|
|
+ sollposition = (totalTurns * prozent * 0.01);
|
|
|
+ Serial.print("sollposition: ");
|
|
|
+ Serial.println(sollposition);
|
|
|
+ prozent = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+}*/
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void loop() {
|
|
|
+
|
|
|
+ /*if(Startup){
|
|
|
+
|
|
|
+
|
|
|
+ Kalibrieren();
|
|
|
+
|
|
|
+ Startup = 0;
|
|
|
+ }
|
|
|
+ //Positionsabfrage();
|
|
|
+ */
|
|
|
+ /* i++;
|
|
|
+ if (i == 50) {
|
|
|
+ Serial.print("NumberofTurns:");
|
|
|
+ Serial.println(numberofTurns);
|
|
|
+ //Serial.print("correctedAngle: ");
|
|
|
+ //Serial.println(correctedAngle, 2);
|
|
|
+ if (speed == 0) {
|
|
|
+ Serial.println("In Sollposition");
|
|
|
+ } else if (richtung == HIGH && speed == 255) {
|
|
|
+ Serial.println("Bewegung nach Unten");
|
|
|
+ } else if (richtung == LOW && speed == 255) {
|
|
|
+ Serial.println("Bewegung nach Oben");
|
|
|
+ }
|
|
|
+ i = 0;*/
|
|
|
+
|
|
|
+ if (Serial.available()){
|
|
|
+ for(int n = 0; n < 10 ; n++){
|
|
|
+ val[n] = Serial.read() - 48; //convert ASCII to int and assigns into array
|
|
|
+ }
|
|
|
+ position = 0;
|
|
|
+ for (int i = 2; i < 5; i++) {
|
|
|
+ position = position * 10 + val[i];
|
|
|
+ }
|
|
|
+ choice = val[0];
|
|
|
+ Serial.print("Motor: ");
|
|
|
+ Serial.print(choice);
|
|
|
+ Serial.print(" position: ");
|
|
|
+ Serial.print(position);
|
|
|
+ switch (val[0]) {
|
|
|
+ case 1:
|
|
|
+ ax12_1(position);
|
|
|
+ Serial.print("Ready");
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ a116(position);
|
|
|
+ Serial.print("Ready");
|
|
|
+ break;
|
|
|
+ case 3:
|
|
|
+ ax12_2(position);
|
|
|
+ Serial.print("Ready");
|
|
|
+ break;
|
|
|
+ case 4:
|
|
|
+ //motor(position);
|
|
|
+ prozent = position;
|
|
|
+ sollposition = (totalTurns * prozent * 0.01);
|
|
|
+ Serial.print("sollposition: ");
|
|
|
+ Serial.println(sollposition);
|
|
|
+ Serial.print("Ready");
|
|
|
+ break;
|
|
|
+ case 5:
|
|
|
+ //auto_close();
|
|
|
+ Serial.print("Ready");
|
|
|
+ break;
|
|
|
+ case 6:
|
|
|
+ Kalibrieren();
|
|
|
+ Serial.print("Ready");
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ if (sollposition > numberofTurns) {
|
|
|
+ richtung = LOW;
|
|
|
+ speed = 255;
|
|
|
+ //Serial.println("Bewegung nach Oben");
|
|
|
+ } else if (sollposition < numberofTurns) {
|
|
|
+ richtung = HIGH;
|
|
|
+ speed = 255;
|
|
|
+ //Serial.println("Bewegung nach Unten");
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ speed = 0;
|
|
|
+ //Serial.println("In Sollposition");
|
|
|
+
|
|
|
+ }
|
|
|
+ digitalWrite(dir,richtung);
|
|
|
+ analogWrite(pwma,speed);
|
|
|
+ delay(50);
|
|
|
+ ReadRawAngle(); //ask the value from the sensor
|
|
|
+ correctAngle(); //tare the value
|
|
|
+ checkQuadrant(); //check quadrant, check rotations, check absolute angular position
|
|
|
+ i++;
|
|
|
+ if (i == 50) {
|
|
|
+ Serial.print("NumberofTurns:");
|
|
|
+ Serial.println(numberofTurns);
|
|
|
+ //Serial.print("correctedAngle: ");
|
|
|
+ //Serial.println(correctedAngle, 2);
|
|
|
+ if (speed == 0) {
|
|
|
+ Serial.println("In Sollposition");
|
|
|
+ } else if (richtung == HIGH && speed == 255) {
|
|
|
+ Serial.println("Bewegung nach Unten");
|
|
|
+ } else if (richtung == LOW && speed == 255) {
|
|
|
+ Serial.println("Bewegung nach Oben");
|
|
|
+ }
|
|
|
+ i = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void ax12_1(int position){
|
|
|
+ if((position<min_angle) || (position>max_angle)){
|
|
|
+ Serial.print("invalide position. Try again");
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ ax12a.moveSpeed(ID1,position, playtime);
|
|
|
+}
|
|
|
+
|
|
|
+void ax12_2(int position){
|
|
|
+ if((position<min_angle) || (position>max_angle)){
|
|
|
+ Serial.print("invalide position. Try again");
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ ax12a.moveSpeed(ID2,position, playtime);
|
|
|
+}
|
|
|
+
|
|
|
+void a116(int position){
|
|
|
+ if((position<min_angle) || (position>max_angle)){
|
|
|
+ Serial.print("invalide position. Try again");
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ servo1.setPosition(position, playtime);
|
|
|
+}
|
|
|
+
|
|
|
+void Kalibrieren(){
|
|
|
+ Serial.print("Start Kalibrieren");
|
|
|
+ digitalWrite(dir,HIGH);
|
|
|
+ analogWrite(pwma,255);
|
|
|
+ Serial.println(digitalRead(button_unten));
|
|
|
+ while(digitalRead(button_unten)){
|
|
|
+ ReadRawAngle(); //ask the value from the sensor
|
|
|
+ correctAngle(); //tare the value
|
|
|
+ checkQuadrant();
|
|
|
+ digitalWrite(dir,HIGH);
|
|
|
+ analogWrite(pwma,255);
|
|
|
+ }
|
|
|
+
|
|
|
+ Serial.println("Archieved 0");
|
|
|
+ numberofTurns = 0;
|
|
|
+
|
|
|
+ while(digitalRead(button_oben)){
|
|
|
+ ReadRawAngle(); //ask the value from the sensor
|
|
|
+ correctAngle(); //tare the value
|
|
|
+ checkQuadrant();
|
|
|
+ digitalWrite(dir,LOW);
|
|
|
+ analogWrite(pwma,255);
|
|
|
+ }
|
|
|
+
|
|
|
+ totalTurns = numberofTurns;
|
|
|
+ Serial.print("TotalTurns:");
|
|
|
+ Serial.println(totalTurns);
|
|
|
+
|
|
|
+ sollposition = totalTurns * 0.5;
|
|
|
+ while(numberofTurns != sollposition) {
|
|
|
+ digitalWrite(dir,HIGH);
|
|
|
+ analogWrite(pwma,255);
|
|
|
+ ReadRawAngle(); //ask the value from the sensor
|
|
|
+ correctAngle(); //tare the value
|
|
|
+ checkQuadrant();
|
|
|
+ }
|
|
|
+
|
|
|
+ Serial.println("Kalib end");
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void ReadRawAngle()
|
|
|
+{
|
|
|
+ //7:0 - bits
|
|
|
+ Wire.beginTransmission(0x36); //connect to the sensor
|
|
|
+ Wire.write(0x0D); //figure 21 - register map: Raw angle (7:0)
|
|
|
+ Wire.endTransmission(); //end transmission
|
|
|
+ Wire.requestFrom(0x36, 1); //request from the sensor
|
|
|
+
|
|
|
+ while(Wire.available() == 0); //wait until it becomes available
|
|
|
+ lowbyte = Wire.read(); //Reading the data after the request
|
|
|
+
|
|
|
+ //11:8 - 4 bits
|
|
|
+ Wire.beginTransmission(0x36);
|
|
|
+ Wire.write(0x0C); //figure 21 - register map: Raw angle (11:8)
|
|
|
+ Wire.endTransmission();
|
|
|
+ Wire.requestFrom(0x36, 1);
|
|
|
+
|
|
|
+ while(Wire.available() == 0);
|
|
|
+ highbyte = Wire.read();
|
|
|
+
|
|
|
+ highbyte = highbyte << 8; //shifting to left
|
|
|
+
|
|
|
+ rawAngle = highbyte | lowbyte; //int is 16 bits (as well as the word)
|
|
|
+ //360/4096 = 0.087890625
|
|
|
+ degAngle = rawAngle * 0.087890625;
|
|
|
+
|
|
|
+ //Serial.print("Deg angle: ");
|
|
|
+ //Serial.println(degAngle, 2); //absolute position of the encoder within the 0-360 circle
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void correctAngle()
|
|
|
+{
|
|
|
+ //recalculate angle
|
|
|
+ correctedAngle = degAngle - startAngle; //this tares the position
|
|
|
+
|
|
|
+ if(correctedAngle < 0) //if the calculated angle is negative, we need to "normalize" it
|
|
|
+ {
|
|
|
+ correctedAngle = correctedAngle + 360; //correction for negative numbers (i.e. -15 becomes +345)
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //do nothing
|
|
|
+ }
|
|
|
+ //Serial.print("Corrected angle: ");
|
|
|
+ //Serial.println(correctedAngle, 2); //print the corrected/tared angle
|
|
|
+}
|
|
|
+
|
|
|
+void checkQuadrant()
|
|
|
+{
|
|
|
+ /*
|
|
|
+ //Quadrants:
|
|
|
+ 4 | 1
|
|
|
+ ---|---
|
|
|
+ 3 | 2
|
|
|
+ */
|
|
|
+
|
|
|
+ //Quadrant 1
|
|
|
+ if(correctedAngle >= 0 && correctedAngle <=90)
|
|
|
+ {
|
|
|
+ quadrantNumber = 1;
|
|
|
+ }
|
|
|
+
|
|
|
+ //Quadrant 2
|
|
|
+ if(correctedAngle > 90 && correctedAngle <=180)
|
|
|
+ {
|
|
|
+ quadrantNumber = 2;
|
|
|
+ }
|
|
|
+
|
|
|
+ //Quadrant 3
|
|
|
+ if(correctedAngle > 180 && correctedAngle <=270)
|
|
|
+ {
|
|
|
+ quadrantNumber = 3;
|
|
|
+ }
|
|
|
+
|
|
|
+ //Quadrant 4
|
|
|
+ if(correctedAngle > 270 && correctedAngle <360)
|
|
|
+ {
|
|
|
+ quadrantNumber = 4;
|
|
|
+ }
|
|
|
+ //Serial.print("Quadrant: ");
|
|
|
+ //Serial.println(quadrantNumber); //print our position "quadrant-wise"
|
|
|
+
|
|
|
+ if(quadrantNumber != previousquadrantNumber) //if we changed quadrant
|
|
|
+ {
|
|
|
+ if(quadrantNumber == 1 && previousquadrantNumber == 4)
|
|
|
+ {
|
|
|
+ numberofTurns--; // 4 --> 1 transition: CW rotation
|
|
|
+ }
|
|
|
+
|
|
|
+ if(quadrantNumber == 4 && previousquadrantNumber == 1)
|
|
|
+ {
|
|
|
+ numberofTurns++; // 1 --> 4 transition: CCW rotation
|
|
|
+ }
|
|
|
+ //this could be done between every quadrants so one can count every 1/4th of transition
|
|
|
+
|
|
|
+ previousquadrantNumber = quadrantNumber; //update to the current quadrant
|
|
|
+
|
|
|
+ }
|
|
|
+ //Serial.print("Turns: ");
|
|
|
+ //Serial.println(numberofTurns,0); //number of turns in absolute terms (can be negative which indicates CCW turns)
|
|
|
+
|
|
|
+ //after we have the corrected angle and the turns, we can calculate the total absolute position
|
|
|
+ totalAngle = (numberofTurns*360) + correctedAngle; //number of turns (+/-) plus the actual angle within the 0-360 range
|
|
|
+ //Serial.print("Total angle: ");
|
|
|
+ //Serial.println(totalAngle, 2); //absolute position of the motor expressed in degree angles, 2 digits
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void checkMagnetPresence()
|
|
|
+{
|
|
|
+ //This function runs in the setup() and it locks the MCU until the magnet is not positioned properly
|
|
|
+
|
|
|
+ while((magnetStatus & 32) != 32) //while the magnet is not adjusted to the proper distance - 32: MD = 1
|
|
|
+ {
|
|
|
+ magnetStatus = 0; //reset reading
|
|
|
+
|
|
|
+ Wire.beginTransmission(0x36); //connect to the sensor
|
|
|
+ Wire.write(0x0B); //figure 21 - register map: Status: MD ML MH
|
|
|
+ Wire.endTransmission(); //end transmission
|
|
|
+ Wire.requestFrom(0x36, 1); //request from the sensor
|
|
|
+
|
|
|
+ while(Wire.available() == 0); //wait until it becomes available
|
|
|
+ magnetStatus = Wire.read(); //Reading the data after the request
|
|
|
+
|
|
|
+ Serial.print("Magnet status: ");
|
|
|
+ Serial.println(magnetStatus, BIN); //print it in binary so you can compare it to the table (fig 21)
|
|
|
+ }
|
|
|
+
|
|
|
+ //Status register output: 0 0 MD ML MH 0 0 0
|
|
|
+ //MH: Too strong magnet - 100111 - DEC: 39
|
|
|
+ //ML: Too weak magnet - 10111 - DEC: 23
|
|
|
+ //MD: OK magnet - 110111 - DEC: 55
|
|
|
+
|
|
|
+ //Serial.println("Magnet found!");
|
|
|
+ //delay(1000);
|
|
|
+}
|