Issues configuring haptic feedback

, ,
1

I have a little confused with how I can correctly set up haptic feedback. I am using a torque controller with dc current. When I use a PID controller with a pretty low P and 0 for I and D I get a soft force at the attract angles which is preferred. The issues im having is that the motor bounces back and forth.

I tried setting torque to 0 when the velocity is too high (during debugging I saw that the velocity reading is maxing out at about 12). If I use a treshold closer to the limit, it does nothing to stop the bouncing. If I set the threshold to a lower value, then the velocity reading just when I am physically moving the motor, surpases that limit and then I lose the haptic feedback.

I tried multiplying the target value by a percentage of the velocity relative to the max velocity. Basically If its spinning fast, then dont apply torque.
This approach almost works, it does reduce wobbles, sometime completely stopping them, but again I feel no feedback if I move too fast, and the reductions in wobbles is not successful when I use higher torque (a higher P value in the controller)

Can anyone point me in the right direction of how I should go about this?

This is my code:

#include <Arduino.h>
#include "./my_functions.h"
#include "Adafruit_GC9A01A.h"
#include "Wire.h"
#include <Adafruit_INA219.h>
#include <SimpleFOC.h>

/*-----------------------------------------------
  Current Sensor
-----------------------------------------------*/
// the current sensor
#define ma_sensor_addr_red_C (0x40)	   // Red motor wirie - Phase C
#define ma_sensor_addr_yellow_A (0x41) // Yellow motor wire - Phase A
#define ma_sensor_addr_blue_B (0x44)   // Blue motor wire  - phase B
TwoWire currentSenseI2C(0);
Adafruit_INA219 current_sense_yellow_A(ma_sensor_addr_yellow_A);
Adafruit_INA219 current_sense_blue_B(ma_sensor_addr_blue_B);
Adafruit_INA219 current_sense_red_C(ma_sensor_addr_red_C);
#define cur_SDA_PIN 2
#define cur_SCL_PIN 1

float current_mA = 0;

// current sensor (custom modification to work work i2c current sensing instead of analog)
InlineCurrentSense current_sense = InlineCurrentSense(&current_sense_red_C, &current_sense_blue_B, &current_sense_yellow_A);

/*-----------------------------------------------
  Motor Driver
-----------------------------------------------*/
#define UL 14 // red motor wire
#define UH 13 // red motor wire  - phase C
#define VL 12 // Blue motor wire
#define VH 21 // Blue motor wire -   pahse B
#define WL 47 // yellow motor wire
#define WH 48 // yellow motor wire - phase A
BLDCDriver6PWM driver = BLDCDriver6PWM(WH, WL, VH, VL, UH, UL);

BLDCMotor motor = BLDCMotor(7); //, 7.8, 270);

/*-----------------------------------------------
  Position Sensor
-----------------------------------------------*/
MagneticSensorI2C as5600 = MagneticSensorI2C(AS5600_I2C);
TwoWire positionSensorI2C(1);
#define pos_SDA_PIN 17
#define pos_SCL_PIN 16


/*
	Haptic controller and variables
*/
PIDController P_haptic(0.25, 0, 0, 100000, 5);
// attractor angle variable
float attract_angle = 0;
// distance between attraction points
float attractor_distance = (45 * 3.14159265359f) / 180.0; // dimp each 45 degrees
float findAttractor(float current_angle)
{
	return round(current_angle / attractor_distance) * attractor_distance;
}

void setup()
{
	Serial.begin(115200);

	/*
		initialize and connect the position sensor
	*/
	positionSensorI2C.begin(pos_SDA_PIN, pos_SCL_PIN);
	as5600.init(&positionSensorI2C);
	motor.linkSensor(&as5600);

	/*
		initialize the current sensors
	*/
	currentSenseI2C.begin(cur_SDA_PIN, cur_SCL_PIN); // SDA - SCL

	//Driver setup
	driver.voltage_power_supply = 5;
	driver.init();
	motor.linkDriver(&driver);
	motor.voltage_sensor_align = 5;

	motor.phase_resistance = 7.8;							 // [Ohm]
	motor.current_limit = 1.2;								 // [Amps] - if phase resistance defined
	motor.voltage_limit = 5;								 // [V] - if phase resistance not defined
	motor.velocity_limit = 20;								 // [rad/s] cca 50rpm
	motor.torque_controller = TorqueControlType::dc_current; // TorqueControlType::voltage;
	motor.controller = MotionControlType::torque;

	current_sense.linkDriver(&driver);
	current_sense.init();
	motor.linkCurrentSense(&current_sense);

	motor.init();
	motor.initFOC();
	motor.target = 0;

	Serial.println("Motor ready!");
	Serial.println("Set target velocity [rad/s]");
	_delay(1000);
}

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
float maxVelocity = 0;
float targetTorque = 0;
void loop()
{
	motor.loopFOC();
	maxVelocity = 13; //I noticed when using a P of 0.25, the max veltocity was about 13
	//if the velocity is at 0, apply full torque, if the velocity is at the max I saw, apply no torque
	targetTorque = mapFloat((fabsf(motor.shaft_velocity) / maxVelocity), 0, 1, 1, 0) * 
        P_haptic(attract_angle - motor.shaft_angle);
	motor.move(targetTorque);
	attract_angle = findAttractor(motor.shaft_angle);
}
2

Can you try without current sense?
I2C may be too slow, current sensing needs to happen very quickly inside FOC loop. On top of this, usually there is not a lot of benefit to current sensing on these gimbal type motors used for haptics due to their high phase resistance, startup currents are much lower.

Are you using the sparkfun haptics development board? Or something custom?

3

Im using an esp32-s3. So I should just use “TorqueControlType::voltage”
Should I remove the code about the velocity and just use the
P_haptic(attract_angle - motor.shaft_angle)
as the motor target?
Should I still use
motor.controller = MotionControlType::torque;
or a different one? maybe angle?

4

well Good lord, it was really that easy, I changed to torque/voltage and left out the velocity part and it works amazing, thank you so much

1 Like
5

Awesome to hear!
If you want to join the discord, there is a channel there for haptics, myself and some others post about what we make.

1 Like