TeensyStep - Fallen at the first hurdle!

Status
Not open for further replies.
Glad you found it. Please use the DevTimer branch. Having the controller locally would require a major restructuring of your code which is not worth it. I'll merge this branch into master asap anyway.
 
Hi Luni,

Right I've got the startup homing sweep working, but now the rotate controllers don't respond - do I need to somehow release the step controller?

Code:
#include "TeensyStep.h"
#include <AnalogSmooth.h>

//------Steppers--------------
constexpr float P = 0.01;             // (P)roportional constant of the regulator needs to be adjusted (depends on speed and acceleration setting)

constexpr unsigned Stepper1_Interval = 10; // ms
constexpr unsigned Stepper2_Interval = 10; // ms
constexpr unsigned Stepper3_Interval = 10; // ms
constexpr unsigned Stepper4_Interval = 10; // ms

const int RESET = 18; // pin for RESET

Stepper Stepper1(5, 6);
Stepper Stepper2(7, 8);
Stepper Stepper3(11, 12);
Stepper Stepper4(9, 10);
RotateControl controller1;
RotateControl controller2;
RotateControl controller3;
RotateControl controller4;

StepControl StepC;

int32_t target = 0;
int32_t target2 = 0;
int32_t target3 = 0;
int32_t target4 = 0;

void Stepper1Update()
{
  static unsigned lastTick = 0;

  if (millis() - lastTick > Stepper1_Interval)
  {
    float delta = (target - Stepper1.getPosition()) * (P / Stepper1_Interval);  // This implements a simple P regulator (can be extended to a Stepper1 if necessary)
    float factor = std::max(-1.0f, std::min(1.0f, delta)); // limit to -1.0..1.0

    controller1.overrideSpeed(factor); // set new speed
    lastTick = 0;
  }
}

void Stepper2Update()
{
  static unsigned lastTick = 0;

  if (millis() - lastTick > Stepper2_Interval)
  {
    float delta = (target2 - Stepper2.getPosition()) * (P / Stepper2_Interval);  // This implements a simple P regulator (can be extended to a Stepper1 if necessary)
    float factor = std::max(-1.0f, std::min(1.0f, delta)); // limit to -1.0..1.0

    controller2.overrideSpeed(factor); // set new speed
    lastTick = 0;
  }
}

void Stepper3Update()
{
  static unsigned lastTick = 0;

  if (millis() - lastTick > Stepper3_Interval)
  {
    float delta = (target3 - Stepper3.getPosition()) * (P / Stepper3_Interval);  // This implements a simple P regulator (can be extended to a Stepper1 if necessary)
    float factor = std::max(-1.0f, std::min(1.0f, delta)); // limit to -1.0..1.0

    controller3.overrideSpeed(factor); // set new speed
    lastTick = 0;
  }
}

void Stepper4Update()
{
  static unsigned lastTick = 0;

  if (millis() - lastTick > Stepper4_Interval)
  {
    float delta = (target4 - Stepper4.getPosition()) * (P / Stepper4_Interval);  // This implements a simple P regulator (can be extended to a Stepper1 if necessary)
    float factor = std::max(-1.0f, std::min(1.0f, delta)); // limit to -1.0..1.0

    controller4.overrideSpeed(factor); // set new speed
    lastTick = 0;
  }
}

//----------------------END STEPPERS------------------------------

//Milli Time -----------------------------
unsigned long currentMillis = 0;
unsigned long Prev_Time_Pot = 0;      // will store last time LED was updated
unsigned long Pot_Interval = 10;          // interval at which to blink (milliseconds
unsigned long Time_Millis = 0;
unsigned long Serial_Interval = 00;
unsigned long Prev_Time_Serial = 0;

//Speed-----------------------------------------
volatile unsigned long VSS_count = 0;
unsigned long VSS_prior_count = 0;
int pin_VSS = 14;
int SPEED_MPH;
int SPEED_STEPS;
unsigned long SpeedLastMicro;
unsigned long VSS_new_count;
int Speed_Teeth = 17;
float diffRatio = 3.42;
int WheelCirc = 2126; //mm
float GearboxToothGap = (1.0 / (Speed_Teeth));
float TyreTravelled = WheelCirc / diffRatio;
unsigned long SpeedTimeCount;
unsigned long speedRaw;
int Speed_Pulses = 0;
unsigned long SpeedTime;
//---------------------------------------------

//RPM
//Speed-----------------------------------------
volatile unsigned long RPM_count = 0;
unsigned long RPM_prior_count = 0;
int pin_RPM = 15;
int RPM;
int RPM_STEPS;
unsigned long RPMLastMicro;
unsigned long RPM_new_count;
int RPM_Teeth = 4;
unsigned long RPMTimeCount;
unsigned long RPMRaw;
int RPM_Pulses = 0;
unsigned long RPMTime;
//---------------------------------------------

//Coolant_T
int CT_Pin = 17;
int CT_STEPS;
int COOLANT_TEMP = 0;

//Fuel_L
int FL_Pin = 16;
int FL_STEPS;
int FUEL_LEVEL = 0;


//Pot Damper -----------------------------
float damping_coefficient = 0.01;
float Hz = 0;
int i = 1;
// Defaults to window size 10
AnalogSmooth as = AnalogSmooth();

// Window size can range from 1 - 100
AnalogSmooth as100 = AnalogSmooth(50);
AnalogSmooth as101 = AnalogSmooth(50);
//----------------------------------------

//Serial
char rc;
String readString;
int StrS1;
int StrF1;
int StrS2;
int StrF2;
int StrS3;
int StrF3;
int StrS4;
int StrF4;
String Pkt1;
String Pkt2;
String Pkt3;
String Pkt4;
String inChar;
int SPEED_SEND;
int RPM_SEND;
int CT_SEND;
int FL_SEND;

void setup()
{
  Serial.begin(9600);
  Serial1.begin(250000);
  pinMode(RESET, OUTPUT);

  //-------------Initiate stepper driver----------------
  digitalWrite(RESET, LOW);
  delay(1);  // keep reset low min 1ms
  digitalWrite(RESET, HIGH);


  //---------------------STEPPER HOMING CODE GOES HERE---------------------------
  Stepper1
  .setMaxSpeed(10000)
  .setAcceleration(75000)
  .setTargetRel(3700);
  
  Stepper2
  .setMaxSpeed(10000)
  .setAcceleration(75000)
  .setTargetRel(3700);
  
  Stepper3
  .setMaxSpeed(10000)
  .setAcceleration(75000)
  .setTargetRel(3700);
  
  Stepper4
  .setMaxSpeed(10000)
  .setAcceleration(75000)
  .setTargetRel(3700);

  StepC.move(Stepper1,Stepper2,Stepper3,Stepper4);

  Stepper1.setTargetRel(-3800);
  Stepper2.setTargetRel(-3800);
  Stepper3.setTargetRel(-3800);
  Stepper4.setTargetRel(-3800);

  StepC.move(Stepper1,Stepper2,Stepper3,Stepper4);
  
  
  //Speed Setup---------------------------------------------------
  GearboxToothGap = GearboxToothGap * TyreTravelled;
  pinMode(pin_VSS, INPUT_PULLUP);
  pinMode(pin_RPM, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(pin_VSS), SpeedPulse, RISING);
  attachInterrupt(digitalPinToInterrupt(pin_RPM), RPMPulse, RISING);
  NVIC_SET_PRIORITY(IRQ_PORTA, 0);
  //---------------------------------------------------

}

void loop() {
  noInterrupts ();
  VSS_new_count = VSS_count;
  RPM_new_count = RPM_count;
  interrupts ();
  SpeedTime = micros();
  RPMTime = micros();

  //----Speed Pulse Read----------
  Speed_Pulses = VSS_new_count - VSS_prior_count;
  if (Speed_Pulses >= 10) {
    SpeedTimeCount = (SpeedTime - SpeedLastMicro) / 1000;
    if (SpeedTimeCount > 400) { //---------------------------------MAY NEED TO CHANGE THIS TO LOCK IN 0
      speedRaw = 0;
    }
    else {
      speedRaw = ((VSS_new_count - VSS_prior_count) * 60 * 1000 / (GearboxToothGap * SpeedTimeCount)) / 10;
    }

    VSS_prior_count = VSS_new_count;
    SpeedLastMicro = SpeedTime;
  }

  //----RPM Pulse Read----------
  //  RPM_Pulses = RPM_new_count - RPM_prior_count;
  //  if (RPM_Pulses >= 4) {
  //    RPMTimeCount = (RPMTime - RPMLastMicro) / 1000;
  //    RPMRaw = 1000000 / RPMTimeCount;
  //    RPM_prior_count = RPM_new_count;
  //    RPMLastMicro = RPMTime;
  //  }


  //------Get Fuel & Temp Data

  //Serial.println(SpeedTime);
  currentMillis = millis();
  Time_Millis = currentMillis - Prev_Time_Pot;
  if (Time_Millis > Pot_Interval)
  {


    //=========OR READ POTS========
    //      COOLANT_TEMP = as100.analogReadSmooth(CT_Pin) / 10;
    //      //COOLANT_TEMP = 81798 / CT_Val; //Define line curve here
    //      COOLANT_TEMP = map(CT_Pin, 10, 1023, 20, 150);        // map temp
    //
    //      FUEL_LEVEL = as101.analogReadSmooth(FL_Pin) / 10;
    //      //FUEL_LEVEL = 81798 / FL_Val; //define line curve here
    //      FUEL_LEVEL = map(FL_Pin, 10, 1023, 0, 57);        // map fuel level

    Prev_Time_Pot = currentMillis;
    Prev_Time_Serial = currentMillis;
  }
    //=========READ SERIAL=========
    Get_Serial_Data();

  //=====SPEED CALC=============
  SPEED_MPH = speedRaw * 1.6;  //speed conversion
  SPEED_STEPS = map(SPEED_MPH, 0, 200, 0, 3000);

  //=====RPM CALC=============
  RPM_STEPS = map(RPM, 0, 7000, 0, -2835);

  //=====CT CALC=============
  CT_STEPS = map(COOLANT_TEMP, 20, 150, 0, 2216);

  //=====FL CALC=============
  FL_STEPS = map(FUEL_LEVEL, 0, 57, -2216, 0);


  //=======MOVE STEPPERS=================
  Stepper1Update();
  Stepper2Update();
  Stepper3Update();
  Stepper4Update();

  target = SPEED_STEPS;    //random(-3000, 3000);
  target2 = RPM_STEPS;    //random(-3000, 3000);
  target3 = CT_STEPS;    //random(-3000, 3000);
  target4 = FL_STEPS;    //random(-3000, 3000);

  //Send Serial Data=============================

  currentMillis = millis();
  Time_Millis = currentMillis - Prev_Time_Serial;
  if (Time_Millis > Serial_Interval)
  {
    //noInterrupts ();
    Send_Serial_Data();
    //interrupts ();
    Serial.println(SpeedTimeCount);
  }
  //Serial.println(RPM);
}
//================================END OF LOOP=============================

//================================FUNCTIONS===============================
void SpeedPulse() {
  VSS_count = VSS_count + 1;
}

void RPMPulse() {
  RPM_count = RPM_count + 1;
}

void Get_Serial_Data() {
  //inChar = Serial1.read();

  if (Serial1.available() > 0) {
    rc = Serial1.read();
    if (rc == '>') {
      //do stuff

      //Serial.println();
      //Serial.print("captured String is : ");
      //Serial.println(readString); //prints string to serial port out

      if (readString.indexOf("S") > 0) {  //is speed in the data packet?
        StrS1 = readString.indexOf('S');  //finds start location
        StrF1 = readString.lastIndexOf('S'); //finds finish location
        Pkt1 = readString.substring(StrS1 + 1, StrF1); //captures data String
      }

      if (readString.indexOf("R") > 0) {  //is speed in the data packet?
        StrS2 = readString.indexOf('R');  //finds start location
        StrF2 = readString.lastIndexOf('R'); //finds finish location
        Pkt2 = readString.substring(StrS2 + 1, StrF2); //captures data String
      }

      if (readString.indexOf("T") > 0) {  //is speed in the data packet?
        StrS3 = readString.indexOf('T');  //finds start location
        StrF3 = readString.lastIndexOf('T'); //finds finish location
        Pkt3 = readString.substring(StrS3 + 1, StrF3); //captures data String
      }

      if (readString.indexOf("F") > 0) {  //is speed in the data packet?
        StrS4 = readString.indexOf('F');  //finds start location
        StrF4 = readString.lastIndexOf('F'); //finds finish location
        Pkt4 = readString.substring(StrS4 + 1, StrF4); //captures data String
      }

      //            Serial.print("Speed = ");
      //            Serial.println(Pkt1);
      //            Serial.print("RPM = ");
      //            Serial.println(Pkt2);
      //            Serial.print("Coolant = ");
      //            Serial.println(Pkt3);
      //            Serial.print("Fuel = ");
      //            Serial.println(Pkt4);
      //            Serial.println();
      //            Serial.println();


      readString = ""; //clears variable for new input
      //SPEED_MPH = Pkt1.toInt();
      RPM = Pkt2.toInt() * 10;
      COOLANT_TEMP = Pkt3.toInt(); //Adjust to tune
      FUEL_LEVEL = Pkt4.toInt();
      //Serial.println(CT_STEPS);
      //Serial.println(RPM);
    }
    else {
      readString += rc; //makes the string readString
    }
  }
}

void Send_Serial_Data() {

  //if(SPEED_MPH != SPEED_SEND ||RPM != RPM_SEND ||COOLANT_TEMP != CT_SEND||FUEL_LEVEL != FL_SEND){Serial1.print('<');}

  if (SPEED_MPH != SPEED_SEND) {
    SPEED_SEND = SPEED_MPH;
    Serial1.print('<');
    Serial1.print('S');
    Serial1.print(SPEED_SEND);
    Serial1.print('S');
    Serial1.print('>');
  }
  //Serial.println(SPEED_SEND);
  //  if (RPM != RPM_SEND) {
  //    RPM_SEND = RPM;
  //    Serial1.print('R');
  //    Serial1.print(-RPM_SEND);
  //    Serial1.print('R');
  //  }
  //
  //  if (COOLANT_TEMP != CT_SEND) {
  //    CT_SEND = COOLANT_TEMP;
  //    Serial1.print('T');
  //    Serial1.print(CT_SEND);
  //    Serial1.print('T');
  //  }
  //
  //  if (FUEL_LEVEL != FL_SEND) {
  //    FL_SEND = FUEL_LEVEL;
  //    Serial1.print('F');
  //    Serial1.print(-FL_SEND);
  //    Serial1.print('F');
  //  }

  //if(SPEED_MPH != SPEED_SEND ||RPM != RPM_SEND ||COOLANT_TEMP != CT_SEND||FUEL_LEVEL != FL_SEND){Serial1.print('>');}


}
 
I don't find your starting of the rotation controllers anymore. See #19 where you still have it. Be sure to move it after the sweep code.
 
Thanks Luni that works, but I don't fully understand what that piece of code is doing. All 4 steppers now sweep then return to zero, I have control over the steppers but after starting, stepper 4 shoots to around 2216 steps and I'm not sure why..??
 
Probably a bug? Difficult to remote debug this. Are the other working as expected? I.e. following the input after the sweep?
 
Wait no, it was a bug with the scaling of my input.. It works! Thanks so much Luni, it works so much smoother than Accelstepper! Amazing job. Thanks for your help getting it working. If anyone is interested, here's the code all working:

Code:
#include "TeensyStep.h"
#include <AnalogSmooth.h>

//------Steppers--------------
constexpr float P = 0.01;             // (P)roportional constant of the regulator needs to be adjusted (depends on speed and acceleration setting)

constexpr unsigned Stepper1_Interval = 10; // ms
constexpr unsigned Stepper2_Interval = 10; // ms
constexpr unsigned Stepper3_Interval = 10; // ms
constexpr unsigned Stepper4_Interval = 10; // ms

const int RESET = 18; // pin for RESET

Stepper Stepper1(5, 6);
Stepper Stepper2(7, 8);
Stepper Stepper3(11, 12);
Stepper Stepper4(9, 10);
RotateControl controller1;
RotateControl controller2;
RotateControl controller3;
RotateControl controller4;

StepControl StepC;

int32_t target = 0;
int32_t target2 = 0;
int32_t target3 = 0;
int32_t target4 = 0;

void Stepper1Update()
{
  static unsigned lastTick = 0;

  if (millis() - lastTick > Stepper1_Interval)
  {
    float delta = (target - Stepper1.getPosition()) * (P / Stepper1_Interval);  // This implements a simple P regulator (can be extended to a Stepper1 if necessary)
    float factor = std::max(-1.0f, std::min(1.0f, delta)); // limit to -1.0..1.0

    controller1.overrideSpeed(factor); // set new speed
    lastTick = 0;
  }
}

void Stepper2Update()
{
  static unsigned lastTick = 0;

  if (millis() - lastTick > Stepper2_Interval)
  {
    float delta = (target2 - Stepper2.getPosition()) * (P / Stepper2_Interval);  // This implements a simple P regulator (can be extended to a Stepper1 if necessary)
    float factor = std::max(-1.0f, std::min(1.0f, delta)); // limit to -1.0..1.0

    controller2.overrideSpeed(factor); // set new speed
    lastTick = 0;
  }
}

void Stepper3Update()
{
  static unsigned lastTick = 0;

  if (millis() - lastTick > Stepper3_Interval)
  {
    float delta = (target3 - Stepper3.getPosition()) * (P / Stepper3_Interval);  // This implements a simple P regulator (can be extended to a Stepper1 if necessary)
    float factor = std::max(-1.0f, std::min(1.0f, delta)); // limit to -1.0..1.0

    controller3.overrideSpeed(factor); // set new speed
    lastTick = 0;
  }
}

void Stepper4Update()
{
  static unsigned lastTick = 0;

  if (millis() - lastTick > Stepper4_Interval)
  {
    float delta = (target4 - Stepper4.getPosition()) * (P / Stepper4_Interval);  // This implements a simple P regulator (can be extended to a Stepper1 if necessary)
    float factor = std::max(-1.0f, std::min(1.0f, delta)); // limit to -1.0..1.0

    controller4.overrideSpeed(factor); // set new speed
    lastTick = 0;
  }
}

//----------------------END STEPPERS------------------------------

//Milli Time -----------------------------
unsigned long currentMillis = 0;
unsigned long Prev_Time_Pot = 0;      // will store last time LED was updated
unsigned long Pot_Interval = 10;          // interval at which to blink (milliseconds
unsigned long Time_Millis = 0;
unsigned long Serial_Interval = 00;
unsigned long Prev_Time_Serial = 0;

//Stepper Ranges-------------------
int Stepper1Range = 3000;
int Stepper2Range = -2835;
int Stepper3Range = 2216;
int Stepper4Range = -2216;

//Speed-----------------------------------------
volatile unsigned long VSS_count = 0;
unsigned long VSS_prior_count = 0;
int pin_VSS = 14;
int SPEED_MPH;
int SPEED_STEPS;
unsigned long SpeedLastMicro;
unsigned long VSS_new_count;
int Speed_Teeth = 17;
float diffRatio = 3.42;
int WheelCirc = 2126; //mm
float GearboxToothGap = (1.0 / (Speed_Teeth));
float TyreTravelled = WheelCirc / diffRatio;
unsigned long SpeedTimeCount;
unsigned long speedRaw;
int Speed_Pulses = 0;
unsigned long SpeedTime;
//---------------------------------------------

//RPM
//Speed-----------------------------------------
volatile unsigned long RPM_count = 0;
unsigned long RPM_prior_count = 0;
int pin_RPM = 15;
int RPM;
int RPM_STEPS;
unsigned long RPMLastMicro;
unsigned long RPM_new_count;
int RPM_Teeth = 4;
unsigned long RPMTimeCount;
unsigned long RPMRaw;
int RPM_Pulses = 0;
unsigned long RPMTime;
//---------------------------------------------

//Coolant_T
int CT_Pin = 17;
int CT_STEPS;
int COOLANT_TEMP = 0;

//Fuel_L
int FL_Pin = 16;
int FL_STEPS;
int FUEL_LEVEL = 0;


//Pot Damper -----------------------------
float damping_coefficient = 0.01;
float Hz = 0;
int i = 1;
// Defaults to window size 10
AnalogSmooth as = AnalogSmooth();

// Window size can range from 1 - 100
AnalogSmooth as100 = AnalogSmooth(50);
AnalogSmooth as101 = AnalogSmooth(50);
//----------------------------------------

//Serial
char rc;
String readString;
int StrS1;
int StrF1;
int StrS2;
int StrF2;
int StrS3;
int StrF3;
int StrS4;
int StrF4;
String Pkt1;
String Pkt2;
String Pkt3;
String Pkt4;
String inChar;
int SPEED_SEND;
int RPM_SEND;
int CT_SEND;
int FL_SEND;

void setup()
{
  Serial.begin(9600);
  Serial1.begin(250000);
  pinMode(RESET, OUTPUT);

  //-------------Initiate stepper driver----------------
  digitalWrite(RESET, LOW);
  delay(1);  // keep reset low min 1ms
  digitalWrite(RESET, HIGH);


  //---------------------STEPPER HOMING CODE GOES HERE---------------------------
  Stepper1
  .setMaxSpeed(10000)
  .setAcceleration(75000)
  .setTargetRel(Stepper1Range);

  Stepper2
  .setMaxSpeed(10000)
  .setAcceleration(75000)
  .setTargetRel(Stepper2Range);

  Stepper3
  .setMaxSpeed(10000)
  .setAcceleration(75000)
  .setTargetRel(Stepper3Range);

  Stepper4
  .setMaxSpeed(10000)
  .setAcceleration(75000)
  .setTargetRel(Stepper4Range);

  StepC.move(Stepper1, Stepper2, Stepper3, Stepper4);

  Stepper1.setTargetRel(-Stepper1Range);
  Stepper2.setTargetRel(-Stepper2Range);
  Stepper3.setTargetRel(-Stepper3Range);
  Stepper4.setTargetRel(-Stepper4Range);

  StepC.move(Stepper1, Stepper2, Stepper3, Stepper4);
  //---------------------STEPPER HOMING CODE GOES HERE---------------------------


  
  while (!Serial && millis() < 1000);

  Stepper1
  .setMaxSpeed(10000)
  .setAcceleration(75000);
  controller1.rotateAsync(Stepper1);
  controller1.overrideSpeed(0); // start with stopped slide

  Stepper2
  .setMaxSpeed(10000)
  .setAcceleration(75000);
  controller2.rotateAsync(Stepper2);
  controller2.overrideSpeed(0); // start with stopped slide

  Stepper3
  .setMaxSpeed(10000)
  .setAcceleration(75000);
  controller3.rotateAsync(Stepper3);
  controller3.overrideSpeed(0); // start with stopped slide

  Stepper4
  .setMaxSpeed(10000)
  .setAcceleration(75000);
  controller4.rotateAsync(Stepper4);
  controller4.overrideSpeed(0); // start with stopped slide

  
  //Speed Setup---------------------------------------------------
  GearboxToothGap = GearboxToothGap * TyreTravelled;
  pinMode(pin_VSS, INPUT_PULLUP);
  pinMode(pin_RPM, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(pin_VSS), SpeedPulse, RISING);
  attachInterrupt(digitalPinToInterrupt(pin_RPM), RPMPulse, RISING);
  NVIC_SET_PRIORITY(IRQ_PORTA, 0);
  //---------------------------------------------------

}

void loop() {
  noInterrupts ();
  VSS_new_count = VSS_count;
  RPM_new_count = RPM_count;
  interrupts ();
  SpeedTime = micros();
  RPMTime = micros();

  //----Speed Pulse Read----------
  Speed_Pulses = VSS_new_count - VSS_prior_count;
  if (Speed_Pulses >= 10) {
    SpeedTimeCount = (SpeedTime - SpeedLastMicro) / 1000;
    if (SpeedTimeCount > 400) { //---------------------------------MAY NEED TO CHANGE THIS TO LOCK IN 0
      speedRaw = 0;
    }
    else {
      speedRaw = ((VSS_new_count - VSS_prior_count) * 60 * 1000 / (GearboxToothGap * SpeedTimeCount)) / 10;
    }

    VSS_prior_count = VSS_new_count;
    SpeedLastMicro = SpeedTime;
  }

  //----RPM Pulse Read----------
  //  RPM_Pulses = RPM_new_count - RPM_prior_count;
  //  if (RPM_Pulses >= 4) {
  //    RPMTimeCount = (RPMTime - RPMLastMicro) / 1000;
  //    RPMRaw = 1000000 / RPMTimeCount;
  //    RPM_prior_count = RPM_new_count;
  //    RPMLastMicro = RPMTime;
  //  }


  //------Get Fuel & Temp Data

  //Serial.println(SpeedTime);
  currentMillis = millis();
  Time_Millis = currentMillis - Prev_Time_Pot;
  if (Time_Millis > Pot_Interval)
  {


    //=========OR READ POTS========
    //      COOLANT_TEMP = as100.analogReadSmooth(CT_Pin) / 10;
    //      //COOLANT_TEMP = 81798 / CT_Val; //Define line curve here
    //      COOLANT_TEMP = map(CT_Pin, 10, 1023, 20, 150);        // map temp
    //
    //      FUEL_LEVEL = as101.analogReadSmooth(FL_Pin) / 10;
    //      //FUEL_LEVEL = 81798 / FL_Val; //define line curve here
    //      FUEL_LEVEL = map(FL_Pin, 10, 1023, 0, 57);        // map fuel level

    Prev_Time_Pot = currentMillis;
    Prev_Time_Serial = currentMillis;
  }
  //=========READ SERIAL=========
  Get_Serial_Data();

  //=====SPEED CALC=============
  SPEED_MPH = speedRaw * 1.6;  //speed conversion
  SPEED_STEPS = map(SPEED_MPH, 0, 200, 0, Stepper1Range);

  //=====RPM CALC=============
  RPM_STEPS = map(RPM, 0, 7000, 0, Stepper2Range);

  //=====CT CALC=============
  CT_STEPS = map(COOLANT_TEMP, 20, 150, 0, Stepper3Range);

  //=====FL CALC=============
  FL_STEPS = map(FUEL_LEVEL, 0, 57, 0, Stepper4Range);


  //=======MOVE STEPPERS=================
  Stepper1Update();
  Stepper2Update();
  Stepper3Update();
  Stepper4Update();

  target = SPEED_STEPS;    //random(-3000, 3000);
  target2 = RPM_STEPS;    //random(-3000, 3000);
  target3 = CT_STEPS;    //random(-3000, 3000);
  target4 = FL_STEPS;    //random(-3000, 3000);

  //Send Serial Data=============================

  currentMillis = millis();
  Time_Millis = currentMillis - Prev_Time_Serial;
  if (Time_Millis > Serial_Interval)
  {
    //noInterrupts ();
    Send_Serial_Data();
    //interrupts ();
    Serial.println(SpeedTimeCount);
  }
  //Serial.println(RPM);
}
//================================END OF LOOP=============================

//================================FUNCTIONS===============================
void SpeedPulse() {
  VSS_count = VSS_count + 1;
}

void RPMPulse() {
  RPM_count = RPM_count + 1;
}

void Get_Serial_Data() {
  //inChar = Serial1.read();

  if (Serial1.available() > 0) {
    rc = Serial1.read();
    if (rc == '>') {
      //do stuff

      //Serial.println();
      //Serial.print("captured String is : ");
      //Serial.println(readString); //prints string to serial port out

      if (readString.indexOf("S") > 0) {  //is speed in the data packet?
        StrS1 = readString.indexOf('S');  //finds start location
        StrF1 = readString.lastIndexOf('S'); //finds finish location
        Pkt1 = readString.substring(StrS1 + 1, StrF1); //captures data String
      }

      if (readString.indexOf("R") > 0) {  //is speed in the data packet?
        StrS2 = readString.indexOf('R');  //finds start location
        StrF2 = readString.lastIndexOf('R'); //finds finish location
        Pkt2 = readString.substring(StrS2 + 1, StrF2); //captures data String
      }

      if (readString.indexOf("T") > 0) {  //is speed in the data packet?
        StrS3 = readString.indexOf('T');  //finds start location
        StrF3 = readString.lastIndexOf('T'); //finds finish location
        Pkt3 = readString.substring(StrS3 + 1, StrF3); //captures data String
      }

      if (readString.indexOf("F") > 0) {  //is speed in the data packet?
        StrS4 = readString.indexOf('F');  //finds start location
        StrF4 = readString.lastIndexOf('F'); //finds finish location
        Pkt4 = readString.substring(StrS4 + 1, StrF4); //captures data String
      }

      //            Serial.print("Speed = ");
      //            Serial.println(Pkt1);
      //            Serial.print("RPM = ");
      //            Serial.println(Pkt2);
      //            Serial.print("Coolant = ");
      //            Serial.println(Pkt3);
      //            Serial.print("Fuel = ");
      //            Serial.println(Pkt4);
      //            Serial.println();
      //            Serial.println();


      readString = ""; //clears variable for new input
      //SPEED_MPH = Pkt1.toInt();
      RPM = Pkt2.toInt() * 10;
      COOLANT_TEMP = Pkt3.toInt(); //Adjust to tune
      FUEL_LEVEL = Pkt4.toInt();
      //Serial.println(CT_STEPS);
      //Serial.println(RPM);
    }
    else {
      readString += rc; //makes the string readString
    }
  }
}

void Send_Serial_Data() {

  //if(SPEED_MPH != SPEED_SEND ||RPM != RPM_SEND ||COOLANT_TEMP != CT_SEND||FUEL_LEVEL != FL_SEND){Serial1.print('<');}

  if (SPEED_MPH != SPEED_SEND) {
    SPEED_SEND = SPEED_MPH;
    Serial1.print('<');
    Serial1.print('S');
    Serial1.print(SPEED_SEND);
    Serial1.print('S');
    Serial1.print('>');
  }
  //Serial.println(SPEED_SEND);
  //  if (RPM != RPM_SEND) {
  //    RPM_SEND = RPM;
  //    Serial1.print('R');
  //    Serial1.print(-RPM_SEND);
  //    Serial1.print('R');
  //  }
  //
  //  if (COOLANT_TEMP != CT_SEND) {
  //    CT_SEND = COOLANT_TEMP;
  //    Serial1.print('T');
  //    Serial1.print(CT_SEND);
  //    Serial1.print('T');
  //  }
  //
  //  if (FUEL_LEVEL != FL_SEND) {
  //    FL_SEND = FUEL_LEVEL;
  //    Serial1.print('F');
  //    Serial1.print(-FL_SEND);
  //    Serial1.print('F');
  //  }

  //if(SPEED_MPH != SPEED_SEND ||RPM != RPM_SEND ||COOLANT_TEMP != CT_SEND||FUEL_LEVEL != FL_SEND){Serial1.print('>');}


}
 
Status
Not open for further replies.
Back
Top