#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('>');}
}