/*
One Pole Motor
- Field Coil
- Rotor Coil
- AS5600 hall encoder
- current sensors ACS712x05B
- L298 dual motor bridge
- PiPico MCU

2024-07-09 ch
*/

// L298 pins
#define EnA 16
#define EnB 17
#define In1 21
#define In2 20
#define In3 19
#define In4 18

// current sensors
#define PIN_ROTORCURRENT A1
#define PIN_FIELDCURRENT A0
#define PIN_AS5600ANALOG A2

#define MySerial Serial  // Serial1 is output for pipico debug probe

float VALUE_field = 0;
float VALUE_rotor = 0;
float VALUE_angle = 0;

void setFieldCoil(int16_t value) {
  if (value > 0) {
    digitalWrite(In1, LOW);
    digitalWrite(In2, HIGH);
  } else {
    digitalWrite(In1, HIGH);
    digitalWrite(In2, LOW);
  }
  analogWrite(EnA, abs(value));
}

void setRotorCoil(int16_t value) {
  if (value > 0) {
    digitalWrite(In3, LOW);
    digitalWrite(In4, HIGH);
  } else {
    digitalWrite(In3, HIGH);
    digitalWrite(In4, LOW);
  }
  analogWrite(EnB, abs(value));
}

static uint32_t lastTime;

void setup() {
  //MySerial.begin(115200);
  MySerial.begin(1000000);
  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(In1, OUTPUT);
  pinMode(In2, OUTPUT);
  pinMode(In3, OUTPUT);
  pinMode(In4, OUTPUT);

  analogWriteFreq(1000);
  lastTime = micros();
}

uint16_t iFieldCoil;
uint16_t iRotorCoil;
uint16_t phi;

void showMeasurements() {
  uint32_t dt = micros() - lastTime;
  MySerial.print(dt);
  lastTime = micros();
  MySerial.print(" ");
  MySerial.print(iFieldCoil);
  MySerial.print(" ");
  MySerial.print(iRotorCoil);
  MySerial.print(" ");
  MySerial.println(phi);
}

#define STATE_IDLE 0
#define STATE_MEASURE 1
#define STATE_COMMUTEFIELD 2
#define STATE_COMMUTEROTOR 3

void loop() {
  static uint8_t state = STATE_IDLE;
  if (Serial.available()) {

    uint8_t c = Serial.read();
    switch (c) {
      case 'm':
        {
          state = STATE_MEASURE;
        }
        break;
      case 'f':
        {
          state = STATE_COMMUTEFIELD;
          VALUE_field = 255;
          VALUE_rotor = 180;
          VALUE_angle = 2300;
          digitalWrite(LED_BUILTIN, HIGH);
        }
        break;
      case 'r':
        {
          state = STATE_COMMUTEROTOR;
          VALUE_field = 255;
          VALUE_rotor = 180;
          VALUE_angle = 2300;
          digitalWrite(LED_BUILTIN, HIGH);
        }
        break;
      case ' ':
        {
          state = STATE_IDLE;
          setRotorCoil(0);
          setFieldCoil(0);
          digitalWrite(LED_BUILTIN, LOW);
        }
        break;
        //default: state = STATE_IDLE;
    }
  }

  phi = analogRead(PIN_AS5600ANALOG) * 4;
  phi = (phi + (int16_t)VALUE_angle) & 0xFFF;

  iFieldCoil = analogRead(PIN_FIELDCURRENT);
  iRotorCoil = analogRead(PIN_ROTORCURRENT);

  if (state == STATE_MEASURE) {
    showMeasurements();
  }

  if (state == STATE_COMMUTEFIELD) {
    if (phi > 2048) setFieldCoil(-VALUE_rotor);
    else setFieldCoil(VALUE_rotor);
    setRotorCoil(VALUE_field);
    showMeasurements();
  }

  if (state == STATE_COMMUTEROTOR) {
    if (phi > 2048) setRotorCoil(-VALUE_rotor);
    else setRotorCoil(VALUE_rotor);
    setFieldCoil(VALUE_field);
    showMeasurements();
  }
}