haptic detent with simpleFOC

haptic_lever.ino

// Open loop motor control example for L298N board
#include <SimpleFOC.h>
#include <neotimer.h>

#define IN1 14
#define IN2 12
#define IN3 13
//#define IN4 8

// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number);
BLDCMotor motor = BLDCMotor(7);
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
BLDCDriver3PWM driver = BLDCDriver3PWM(IN1, IN2, IN3);

// encoder instance
Encoder encoder = Encoder(25, 26, 600);

Neotimer debug_timer = Neotimer(500);
Neotimer detent_timer = Neotimer(700);
//int pullup = Pullup::INTERN;

// Interrupt routine intialisation
// channel A and B callbacks
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}

void setup() {
  pinMode(0,OUTPUT);
  pinMode(16,OUTPUT);
  digitalWrite(0,HIGH);
  digitalWrite(16,HIGH);

  encoder.pullup = Pullup::INTERN;
  encoder.init();
  
  encoder.enableInterrupts(doA, doB); 
  motor.linkSensor(&encoder);
  
  // deactivate the OUT4 output
  //pinMode(IN4,OUTPUT);
  //digitalWrite(IN4,LOW);


  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

  
  

   // aligning voltage [V]
  motor.voltage_sensor_align = 3;
  // index search velocity [rad/s]
  motor.velocity_index_search = 3;

  // open loop control config
  motor.controller = ControlType::angle;


  // velocity PI controller parameters
  motor.PID_velocity.P = .3;
  motor.PID_velocity.I = 5;
  // default voltage_power_supply
  motor.voltage_limit = 12;
  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 300;
 
  // velocity low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01;

  // angle P controller
  motor.P_angle.P = 23;
  //  maximal velocity of the position control
  motor.velocity_limit = 320;
  
  // init motor hardware
  Serial.begin(115200);
  motor.useMonitoring(Serial);
  motor.init();
  motor.initFOC();

  
  Serial.println("Motor ready!");
  _delay(1000);
}

float target_velocity = 2; // [rad/s]
// angle set point variable
float target_angle = 0;
float detent = 1.57;
//float detent = 1.05;

void loop() {
  // open loop velocity movement
  // using motor.voltage_limit and motor.velocity_limit
  //motor.move(target_velocity);

  motor.loopFOC();
  /*
  switch (target_angle){
    case 0:
      set_target();
      break;
    case 1.57:
      set_target();
      break;
    case -1.57:
      break;
      
  }
  */
  set_target();
  motor.move(target_angle);
  serialReceiveUserCommand();
  if(debug_timer.repeat()){
    Serial.print(motor.shaft_angle);
    Serial.print(",");
    Serial.print(motor.voltage_q);
    Serial.print(",");
    Serial.print(motor.voltage_d);
    Serial.print(",");
    Serial.print(motor.voltage_limit);

    Serial.println();
    motor.monitor();
    //Serial.p
  }
}

void set_target(){
  if(target_angle == 0 && (motor.shaft_angle <= (detent * .3) && motor.shaft_angle >= (detent * .3 * -1)) && motor.voltage_limit != 5){
    Serial.println("setting voltage limit to 5");
    motor.voltage_limit = 5;
    //return;
  }
  if(target_angle ==  0 && motor.shaft_angle >= (detent * .2) && motor.shaft_angle < (detent *.8) && !detent_timer.waiting()){
    motor.voltage_limit = 5;
    Serial.println("setting detent 1");
    target_angle = detent;
    detent_timer.stop();
    detent_timer.reset();
    detent_timer.start();
    return;
  }
  if(target_angle == detent && motor.shaft_angle > (detent * .2)  && motor.shaft_angle <= (detent * .8) && !detent_timer.waiting()){
    Serial.println("setting detent 0");
    detent_timer.stop();
    detent_timer.reset();
    detent_timer.start();
    target_angle = 0;
  }
  if(detent_timer.done()){
    Serial.println("timer done, resetting");
    detent_timer.stop();
    detent_timer.reset();
  }
}

void serialReceiveUserCommand() {
  
  // a string to hold incoming data
  static String received_chars;
  
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the string buffer:
    received_chars += inChar;
    // end of user input
    if (inChar == '\n') {
      
      // change the motor target
      target_angle = received_chars.toFloat();
      Serial.print("Target angle: ");
      Serial.println(target_angle);
      
      // reset the command buffer 
      received_chars = "";
    }
  }
}