Test rs422 steering sensor

Libraries/STEERING_RS422/STEERING_RS422.h

@@ -0,0 +1,50 @@
+#ifndef __STEERING_RS_422_H__
+#define __STEERING_RS_422_H__
+#include <Arduino.h>
+#include <stdint.h>
+#include <stddef.h>
+
+
+#define DEFAULT_HARDWARE_SERIAL 5
+
+#define MAX_POSITION 8192
+
+class STEERING_RS422 {
+    public: 
+        STEERING_RS422();
+        STEERING_RS422(uint8_t serial);
+        void init(unsigned long baudrate);
+        int16_t read_steering();
+        int16_t read_steering_continuous();
+        void request_status();
+        int16_t get_encoder_position() const { return encoder_position;};
+        void calibrate_steering(uint32_t pos);    
+        void command_sequence();
+        void save_parameters();
+        void continuous_setup(uint16_t period, bool auto_start);
+        void interpret_error_messages(uint8_t status_byte);
+        void continuous_start();
+        void continuous_stop();
+        void reset_sensor();
+        void set_zero_position(uint16_t new_zero_position);
+        void calculate_zero_position();
+        uint16_t return_zero_position();
+        void self_calibration();
+        uint8_t check_calibration_status();
+    private:
+        static uint16_t buf;
+        HardwareSerial* _serial;
+        int16_t encoder_position;
+        bool error;
+        bool warning;
+        uint8_t status;
+        /*
+        r7 - Warning: limit of lower ride height tolerance. Error: Signal Ampltiude too high. Redhead too close to magentor external magnetic field is interfering.
+        r6 - Warning: Limit of upper ride height tolerance. Error: Signal Amplitude too low. Distance bewteen redhead and ring is too large.
+        r5 - Warning: Redhead temp out of range
+        r4 - Warning: Speed too high
+        */
+        uint16_t zero_position;
+};
+
+#endif

Libraries/STEERING_RS422/STEERING_RS_422.cpp

@@ -0,0 +1,245 @@
+#include "STEERING_RS422.h"
+
+// Init STEERING_RS422 using default hardware serial port
+STEERING_RS422::STEERING_RS422() {
+    STEERING_RS422(DEFAULT_HARDWARE_SERIAL);
+}
+
+//Init STEERING_RS422 using configurable hardware serial port
+STEERING_RS422::STEERING_RS422(uint8_t serial) {
+    switch (serial) {
+        case 1:
+            _serial = &Serial1;
+            break;
+        case 2:
+            _serial = &Serial2;
+            break;
+        case 3:
+            _serial = &Serial3;
+            break;
+        case 4:
+            _serial = &Serial4;
+            break;
+        case 5:
+            _serial = &Serial5;
+            break;
+        case 6:
+            _serial = &Serial6;
+            break;
+        case 7:
+            _serial = &Serial7;
+            break;
+        case 8:
+            //_serial = &Serial8; //not on teensy 4.0
+            break;
+    }
+}
+
+void STEERING_RS422::init(unsigned long baudrate) {
+    _serial->begin(baudrate);
+    this->set_zero_position(zero_position);
+}
+
+int16_t STEERING_RS422::read_steering() {
+
+    request_status();
+
+    //check if _serial is available and the first byte is ascii "d"
+
+    return read_steering_continuous();    
+
+}
+
+void STEERING_RS422::request_status() {
+    _serial->write(0x64);
+    delay(1);
+}
+
+int16_t STEERING_RS422::read_steering_continuous() {
+    uint8_t readByte[3] = {0};
+    //check if _serial is available and the first byte is ascii "d"
+    if (_serial->available() >= 4 && _serial->read() == 0x64) {
+        //encoder position left aligned, MSB first
+        readByte[0] = _serial->read(); //encoder position b15-b8 of steering
+        readByte[1] = _serial->read(); //encoder position b7-b0 of steering 
+        readByte[2] = _serial->read(); //detailed status b7-b0
+        encoder_position = (int16_t)(((uint16_t)readByte[0]) << 8) | ((uint16_t)readByte[1]);
+        encoder_position = encoder_position >> 2; //b1-b0 general status
+        uint16_t encoder_position_raw = (((uint16_t)readByte[0]) << 8) | ((uint16_t)readByte[1]);
+        encoder_position_raw = encoder_position_raw >> 2; //b1-b0 general status
+        error = readByte[1] & 2; //if high, current data is not valid, previous data is sent
+        warning = readByte[1] & 1; //if high, some operating conditions are close to limits
+        status = readByte[2];
+        Serial.print("Signed interpretation: ");
+        Serial.println(encoder_position);
+        Serial.print("Encoder raw reading: ");
+        Serial.println(encoder_position_raw);
+
+        interpret_error_messages(status);
+        //_serial->clear();
+    }
+    return encoder_position;   
+}
+
+void STEERING_RS422::interpret_error_messages(uint8_t status_byte) {
+    if (status_byte & 0x80) {
+            Serial.println("Warning: Limit of lower ride height tolerance. Error: Signal amplitude too high.");
+        }
+    if (status_byte & 0x40) {
+        Serial.println("Warning: Limit of upper ride height tolerance. Error: Signal amplitude low.");
+    }
+    if (status_byte & 0x20) {
+        Serial.println("Warning: The readhead temperature is out of specified range.");
+    }
+    if (status_byte & 0x10) {
+        Serial.println("Warning: Speed too high.");
+    }
+    if (status_byte & 0x08) {
+        Serial.println("Error: Multiturn counter error.");
+    }
+}
+
+void STEERING_RS422::set_zero_position(uint16_t new_zero_position) {
+    zero_position = new_zero_position;
+}
+
+void STEERING_RS422::calculate_zero_position() {
+    request_status();
+    delay(5);
+    uint8_t readByte[3] = {0};
+    //check if _serial is available and the first byte is ascii "d"
+    if (_serial->available() >= 4 && _serial->read() == 0x64) {
+        //encoder position left aligned, MSB first
+        readByte[0] = _serial->read(); //encoder position b15-b8 of steering
+        readByte[1] = _serial->read(); //encoder position b7-b0 of steering 
+        readByte[2] = _serial->read(); //detailed status b7-b0
+        set_zero_position(((readByte[0] << 8) | readByte[1]) >> 2);
+        Serial.println(((readByte[0] << 8) | readByte[1]) >> 2);
+    }
+}
+
+uint16_t STEERING_RS422::return_zero_position() {
+    return zero_position;
+}
+
+void STEERING_RS422::calibrate_steering(uint32_t pos) {
+    //start programming sequence
+    //this sends data to essentially add a zero offset
+    Serial.println(pos);
+    command_sequence();
+    //0x5A is programming command byte for position offset setting
+    _serial->write(0x5A);
+    delay(1);
+    _serial->write((pos & 0xFF000000) >> 24);
+    //_serial->write(0);
+    delay(1);
+    _serial->write((pos & 0xFF0000) >> 16);
+    //_serial->write(0);
+    delay(1);
+    _serial->write((pos & 0xFF00) >> 8);
+    //_serial->write(0x2D);
+    delay(1);
+    _serial->write(pos & 0xFF);
+    //_serial->write(0x12);
+    delay(1);
+}
+
+//Unlock sequence for self-calibration, must be followed by programming command byte
+void STEERING_RS422::command_sequence() {
+    delay(1);
+    _serial->write(0xCD);
+    delay(1);
+    _serial->write(0xEF);
+    delay(1);
+    _serial->write(0x89);
+    delay(1);
+    _serial->write(0xAB);
+    delay(1);
+
+}
+
+void STEERING_RS422::continuous_setup(uint16_t period, bool auto_start = 0) {
+    command_sequence();
+    //0x54 is programming command byte for continuous response setting
+    _serial->write(0x54);
+    delay(1);
+    //1 to enable automatic start after power on of encoder, 0 to disable
+    _serial->write((auto_start) ? 0x01 : 0x00);
+    delay(1);
+    _serial->write(0x64);
+    delay(1);
+    //in microseconds
+    _serial->write(period & 0xFF00 >> 8);
+    delay(1);
+    _serial->write(period & 0xFF);
+
+}
+void STEERING_RS422::continuous_start() {
+    command_sequence();
+    //0x53 programming command byte for continuous response start
+    _serial->write(0x53);
+    delay(1);
+}
+void STEERING_RS422::continuous_stop() {
+    command_sequence();
+    //0x50 programming command byte for continuous response stop
+    _serial->write(0x50);
+    delay(1);
+}
+void STEERING_RS422::save_parameters() {
+    command_sequence();
+    //0x63 programming command byte for configuration parameters save
+    _serial->write(0x63);
+    delay(1);
+}
+void STEERING_RS422::reset_sensor() {
+    command_sequence();
+    //0x72 programming command byte for configuration parameters reset
+    _serial->write(0x72);
+    delay(1);
+}
+
+
+void STEERING_RS422::self_calibration() {
+
+    //b6 calibration alr performed
+    //b3 calcualted params out of range
+    //b2 timeout (no complete rotation)
+    //b1 counter bit 1
+    //b0 counter bit 0
+    //check status
+    //check if calubration was already performed
+    //command_sequence();
+    //self calibrate
+    Serial.println("calibrating");
+    command_sequence();
+    _serial->write(0x41);
+    delay(10000); //10 sec delay()
+    Serial.println("calibration complete");
+
+} 
+
+uint8_t STEERING_RS422::check_calibration_status() {
+    uint8_t status_flags = 0;
+    _serial->write(0x69);
+    delay(1);
+    if (_serial->available() && _serial->read() == 0x69) {
+        status_flags = _serial->read();
+    } else {
+        return -1;
+    }
+
+    if (status_flags & 0b01000000) {
+        Serial.println("already calibrated");
+
+    }
+    if (status_flags & 0b00001000) {
+        Serial.println("bad tolerance");
+
+    }
+    if (status_flags & 0b0001100) {
+        Serial.println("errors");
+    }
+    return status_flags;
+
+}

Utilities/steering_rs422_testing/steering_rs422_testing.ino

@@ -4,11 +4,12 @@
 
 STEERING_RS422 steering(5);
 Metro read_steering_timer(20);
+uint8_t valid = -1;
 void setup() {
   // put your setup code here, to run once:
   Serial.begin(115200);
-  Serial5.begin(115200);
-  //steering.init(115200);
+  //Serial5.begin(115200);
+  steering.init(115200);
   //pinMode(13, OUTPUT);
 
   //Comment everything out below reset and reset the sensor. Then comment reset and uncomment everything in 
@@ -22,19 +23,35 @@ void setup() {
   //Uncomment next three lines to set wheel starting position to 0 on every power cycle, must put wheel as straight as possible
   //steering.set_zero_position(0);
   //steering.calculate_zero_position();
-  //steering.calibrate_steering(steering.return_zero_position());
+  //steering.calibrate_
+  //steering(steering.return_zero_position());
   //steering.continuous_setup(32767, 0);
   //Uncomment to save current baud rate, position offset, continuous-response settings (period, command, autostart enable command)
   //steering.save_parameters();
+  //steering.read
 
 
   //Block 2 
   //steering.continuous_start();
+  Serial.println("get ready..");
+  Serial.println();
+  delay(500);
+//
+//  Serial.print("Calibrate offset position to: ");
+//  steering.calibrate_steering(317);
+//  Serial.println("Saving parameter...");
+//  steering.save_parameters();
+
+//  steering.self_calibration();
+
 }
 
 void loop() {
   if (read_steering_timer.check()) {
     //steering.read_steering_continuous();
+    Serial.print("Check self-calibration status (idk why this is here tbh but): ");
+    Serial.println(steering.check_calibration_status(),BIN);
     steering.read_steering();
+    Serial.println();
   }
 }