#include "AS5600.h"
#include "Wire.h"
#include "GyverStepper2.h"
GStepper2<STEPPER2WIRE> stepper(3200, 4, 0, 2);
AS5600 as5600; // use default Wire
bool i2c_print = true;
bool is_Running = false;
static uint32_t lastTime = 0;
char sym;
int32_t val;
int32_t i2c_pos;
int32_t stepp_pos = 0;
int32_t final_pos;
int32_t prev_stepp_pos = 0;
int8_t d = 1;
//Настраиваем адреса и регистры для чтения данных с энкодера по I2C
// Взято отсюда https://russianblogs.com/article/31803346353/
int _ang_hi = 0x0E; // ANGLE
int _ang_lo = 0x0F;
int _raw_ang_hi = 0x0C; //RAW ANGLE
int _raw_ang_lo = 0x0D;
int _ams5600_Address = 0x36;
bool callibration_performance = true;
bool callibration_bool = false;
int32_t value_cal[4096];
hw_timer_t *My_timer = NULL; //для прерываний
class AS56{ //класс в котором происходит чтение данных по i2c с энкодера и преобразуется в абсолютное значение
public:
int AS56_adress = 0;
int counter_sym = 0;
int abs_pos = 0; //
int prev_retVal = 0; //Предыдущий азимут энкодера
int zero_pos = 0;
int retVal = 0; //Текущий азимут энкодера
bool is_Reversed = false;
public:
AS56(int AS56_adress){
this -> AS56_adress = AS56_adress;
}
void reset_Abs_pos(){
int a = this -> readTwoBytes(_raw_ang_hi);
this -> abs_pos = 0;
this -> prev_retVal = 0;
this -> zero_pos = a;
this -> counter_sym = 0;
}
void revolutions( int coner){
if ((coner - this -> prev_retVal) > 1000 && is_Running ){
this -> counter_sym --;}
if ((coner - this -> prev_retVal) < -1000 && is_Running ){
this -> counter_sym ++;
}
this -> prev_retVal = coner;
}
int32_t abs_Pos(){
int a = this -> readTwoBytes(_raw_ang_hi)- this-> zero_pos;
this -> revolutions(a);
uint32_t d = this -> counter_sym*4096 + a;
this -> abs_pos = d;
return (d);
}
int readOneByte(int adress, int val) // Чтение одного байта по I2C
{
int retVal = -1;
Wire.beginTransmission (_ams5600_Address); // начало передачи
Wire.write (adress); // адрес, который вы должны прочитать
Wire.endTransmission();
Wire.requestFrom (_ams5600_Address, 1); // Получить данные из 5600
while(!Wire.available ());
retVal = Wire.read (); // чтение данных
Serial.print("retVal ");
Serial.println(retVal);
this->retVal = retVal;
return retVal;
}
word readTwoBytes(int adress){ // Чтение двух байтов по I2C
int retVal = -1;
Wire.beginTransmission (_ams5600_Address); // начало передачи
Wire.write (adress); // адрес, который вы должны прочитать
Wire.endTransmission();
Wire.requestFrom (_ams5600_Address, 2); // Получить данные из 5600
while(!Wire.available ()); // ожидание автобусной остановки I2C (заканчивается передачей)
word high = Wire.read();
high = high << 8;
int low = Wire.read(); // принять байт как символ
retVal = high | low ; // чтение данных
if (!is_Reversed){
retVal = 4095 - retVal;
}
return retVal;
}
void reverse_Dir(bool i)
{
this->is_Reversed = !i;
}
};
AS56 encod = AS56(783);
void IRAM_ATTR onTimer(){
if (i2c_print == true) {
is_Running = stepper.tick();
}
}
void setup() {
Serial.begin(57600);
Serial.setTimeout(20);
My_timer = timerBegin(0, 80, true); //для прерываний
timerAttachInterrupt(My_timer, &onTimer, true);
timerAlarmWrite(My_timer, 100, true);
timerAlarmEnable(My_timer); //Just Enable
Wire.begin();//для энкодера
Serial.println([B]FILE[/B]);
Serial.print("AS5600_LIB_VERSION: ");
Serial.println(AS5600_LIB_VERSION);
as5600.begin(4); // set direction pin.
as5600.setDirection(AS5600_CLOCK_WISE); // default, just be explicit.
Serial.println(as5600.getAddress());
int b = as5600.isConnected();
Serial.print("Connect: ");
Serial.println(b);
stepper.autoPower(true); //сетап для Gyvera
stepper.setAcceleration(1000);
}
void serial_print(){
lastTime = millis();
i2c_pos = encod.abs_Pos()*(0.78123); //RawAngle //as5600.getCumulativePosition(); readOneByte(_raw_ang_hi);
stepp_pos = -stepper.pos;
Serial.print(stepp_pos); //виртуальная позиция гГивера
Serial.println("\t");
Serial.print(i2c_pos); //позиция энкодера
Serial.print("\t");
}
void loop() {
if (Serial.available() > 0) {
sym = Serial.read();
if (sym == 's') {
String ch = Serial.readString();
String n = ch.substring(1);
val = n.toInt();
if(val > 0) d = 1;
else if (val < 0) d = -1;
if (val ==0) {
е stepper.stop();
}else{
encod.reset_Abs_pos();
stepper.reset();
stepper.setAcceleration(500);
stepper.setTarget(3200*d);
// if (callibration_performance == true) { //
// stepper.setMaxSpeed(100);
// stepper.setTarget(-9999999);
// }
}
}
}
stepp_pos = stepper.pos;
if (val > 1000){
if (millis() - lastTime >= 4 && is_Running) serial_print();
}
else{
if (prev_stepp_pos != stepp_pos && is_Running) serial_print();
}
}
