Read AMS-5048B angle sensor and publish ROS topic for the steering angle

front_angle_v6.cpp

/*

front_angle_v6.cpp

Read the AMS-5048B angle sensor and publish the steering angle - front_angle_raw = the raw value of the sensor

This program will also read an analog potentiometer during setup to make sure the wheels are pointing straight.

The program will track the absolute position of an AMS_AS5048B encoder since CPU reset even though the encoder rolls over the 14bit register 0-16348 
that is built in.  In other words, the encoder can keep spinning and this program keeps track of the total rotations/distance travelled or 
however you want to represent knowing the absolute position.  The value of "ticks" will be stored in "rotational_position".

*/

#include <ros.h>
#include "std_msgs/Int32.h"
#include "std_msgs/Int16.h"
#include <Arduino.h>
#include <Wire.h>

//unit consts
#define AS5048B_ANGLMSB_REG 0xFE //bits 0..7
#define AS5048B_RESOLUTION 16384.0 //14 bits
#define AS5048_ADDRESS 0x40

const int infoInterval = 5000;  // 100 = 1/10 of a second (i.e. 10 Hz) 5000 = 5 seconds
const int readInterval = 100;  // 100 10 HZ, 50 20Hz, 20 50 Hz
const int read_analog_pot_Interval = 1000;  // 100 10 HZ, 50 20Hz, 20 50 Hz, 1000 1 Hz

uint16_t current_position;
uint16_t last_position;
int16_t position_movement;
int32_t rotational_position;

uint16_t bitshift_cur_pos;
uint16_t bitshift_last_pos;
int16_t bitshift_pos_delta;

unsigned long prev_time_stamp_info =  0;
unsigned long prev_time_stamp_read =  0;
unsigned long prev_analog_pot_read =  0;

int wheels_straight_ahead = 0;
int current_position_low_res = 0;

char buffer[250];

ros::NodeHandle nh;
std_msgs::Int32 front_angle_raw;
ros::Publisher as5048b_front_angle_raw("/front_angle_raw", &front_angle_raw);
std_msgs::Int16 front_angle_low_res;
ros::Publisher vishaypot_front_angle_low_res("/front_angle_low_res", &front_angle_low_res);

uint16_t AMS_AS5048B_readReg16() {  //reference: https://github.com/sosandroid/AMS_AS5048B
  byte requestResult;
  byte readArray[2];
  uint16_t readValue = 0;
  Wire.beginTransmission(AS5048_ADDRESS);
  Wire.write(AS5048B_ANGLMSB_REG);
  requestResult = Wire.endTransmission(false);
  if (requestResult){
    Serial.print("I2C error: ");
    Serial.println(requestResult);
  }
  Wire.requestFrom(AS5048_ADDRESS, 2);
  for (byte i=0; i < 2; i++) {
    readArray[i] = Wire.read();
  }

  readValue = (((uint16_t) readArray[0]) << 6);
  readValue += (readArray[1] & 0x3F);
  return readValue;
}

void setup() {
  Wire.begin();
  nh.initNode();
  nh.advertise(as5048b_front_angle_raw);
  nh.advertise(vishaypot_front_angle_low_res);
  pinMode(LED_BUILTIN, OUTPUT);

  while (!nh.connected()){
    nh.spinOnce();
  }
  nh.loginfo("front angle is connected (last compiled 20200614 6:05 pm");

  wheels_straight_ahead = 0;

  while (wheels_straight_ahead == 0) { // check to make sure the front wheels are pointing basically straight ahead
    current_position_low_res = analogRead(A1); //pin 15 on Teensy 3.2
    if (440 <= current_position_low_res && current_position_low_res <= 640) {  // on my numbered gear this is between 14 and 11 - target 13
      wheels_straight_ahead = 1;
      current_position = AMS_AS5048B_readReg16();
      last_position = current_position;
      rotational_position = 0;       
    }
    else{
      nh.spinOnce();
      sprintf (buffer, "front_angle - setup - waiting 10 secs for wheels forward straight - current_position_low_res: %d", current_position_low_res);
      nh.loginfo(buffer);
      delay(10000);
    }
  }
}

void loop() {
  if (millis() - prev_time_stamp_read >= readInterval) {
    current_position = AMS_AS5048B_readReg16();
    //reference: next 5 lines are based on magic code from Jeff Sampson - thank you!
    bitshift_cur_pos = current_position << 2;
    bitshift_last_pos = last_position << 2;
    bitshift_pos_delta = (bitshift_cur_pos - bitshift_last_pos); // Calculate the encoder "ticks" moved since the last reading - in bitshift math
    position_movement = bitshift_pos_delta >> 2; // Calculate the encoder "ticks" moved since the last reading - converting bitshift math
    rotational_position += position_movement; // Update the absolute position values.  
    last_position = current_position; 
    front_angle_raw.data = rotational_position;    // publish ROS topic
    as5048b_front_angle_raw.publish(&front_angle_raw);
    prev_time_stamp_read = millis();
  }

	if (millis() - prev_time_stamp_info >= infoInterval) {  // provide an informational message to ensure all is OK
    sprintf (buffer, "front_angle - rotational_position :%ld , analog position:%d ", rotational_position, current_position_low_res);
		nh.loginfo(buffer);
		prev_time_stamp_info = millis();
	}

  if (millis() - prev_analog_pot_read >= read_analog_pot_Interval) {  
    current_position_low_res = analogRead(A1); //pin 15 on Teensy 3.2
    front_angle_low_res.data = current_position_low_res;    // publish ROS topic
    vishaypot_front_angle_low_res.publish(&front_angle_low_res);    
    prev_analog_pot_read = millis();
  }

	nh.spinOnce();
}