/*-----------------------------------------------------------------------------------------
* This software is in the public domain, furnished "as is", without technical
* support, and with no warranty, express or implied, as to its usefulness for
* any purpose.
*
* Turuntable.ino
*
* Functions to control rotation speed of a turntable with Stepper including 4 modes
* the modes are trigger with the two switches of Switchprop switchOn[S0,S1]
* PropMode [0,0]: Rotation direction and speed controlled with proportional part of
* SwitchProp like an ECU function - runPropTable()
* ServoMode[0,1]: Rotation controlled with proportional part of SwitchProp like a
* Servo from -180 to 180 degree - runServoTable
* RotMode [1,0]: Turntable rotates against the ship rotation and keep its orientation
* If ship is not rotating turntable is orientated to forward - runRotTable()
* SyncMode [1,1]: Turntable set its orientation as ordert with the linear movement direction.
* Rotation will be compensated. If no movement direction is given the
* turntable is orientated to forward - runPosTable(e)
*
* The functions are using the stepper library. This lib must be available to build this binary.
* The step function of the stepper lib is blocking the runtime of other functions,
* therefore this implementation avoids calls of the step function with more then 1 step.
* To get a smooth run of the stepper, the step calls are timed and limited against the
* max. rotation speed of the used stepper (stepSpeed). The definition part for adjustable values
* must be adapted according the technical data sheet of the used stepper.
*
* At Setup a function to set orientation of turntable is used. For this a hallsensor
* at orientation direction of vehicle and a magnet on turntable orientation point is
* needed. Therefor the Hallsensor.ino mast be part of the sketch as well.
*
* Author: Volker Frauenstein
* Date: 09/01/2023 Version: 1.5 function calls for FlashLED functions added in runTable()
*
* Date: 04/12/2022 Version: 1.4 timed stepper calls modified to speed up with 20ms RC loops.
* New and adpted functions: ReStep, PrepDeltaStep, stepNow
* Date: 05/05/2021 Version: 1.3 Mode definitions adapted and functions expanded,
* hallsensor interface and setupTable function adapted
* Date: 22/04/2021 Version: 1.2 interface for LED controll modes (LED_Tmode) added
* Date: 03/04/2021 Version: 1.1 function for position finding of turntable at startup added
* Date: 01/03/2021 Version: 1.0 implementation with L298N H-Bridge and Stepper
*/
// Definitions to handle output to Stepper
#include "Stepper.h"
// definition for pinout to motor driver L298N
const int pinIN1 = 19; // IN1 of L298N stepper driver
const int pinIN2 = 18;
const int pinIN3 = 05;
const int pinIN4 = 17; // IN4 of L298N stepper driver
// definitions for adjustable values
const int stepsPerTurn = 200; // number of steps per revolution (depends on Stepper)
const int stepLimit = stepsPerTurn-1; // step counter starts with 0, therefor limit must be 1 lower as turns
//const int stepSpeed = 160; // rotation speed of stepper per minute (depends on Stepper)
const int stepSpeed = 110; // rotation speed of stepper per minute (depends on Stepper)
const float stepDurationMax = 50; // maximal duration in ms between to steps (bigger value -> slower)
const float deadzoneProp = 0.08; // RC Channel Deadzone of x% positiv values
const float deadzoneNProp = - 0.08; // RC Channel Deadzone of x% negativ values
// definition of stepper object for turntable
Stepper turntable(stepsPerTurn, pinIN1,pinIN2,pinIN3,pinIN4);
// generig definitions
unsigned long turnTimer = 0; // variable for time measurement
// minimal duration in ms between to steps
const float stepDurationMin = 60000 / (float(stepsPerTurn)*float(stepSpeed));
const float stepAngel = 360 / float(stepsPerTurn); // rotation angel per step
const int stepsPerHalf = stepsPerTurn / 2; // number of steps per halfrevolution
int stepDurationHigh = stepDurationMin +1; // buffer for upcomming duration between two steps higher value
int stepDurationLow = stepDurationMin +1; // buffer for upcomming duration between two steps lower value
bool NextStepLow = true; // buffer to toggle between stepDurationHigh and Low
unsigned long TnextStep = 0; // buffer timestamp for next step in 20ms interval
bool c_wise = true; // buffer direction of needed steps in 20ms interval -> true is clockwise
int CycleSteps = 0; // buffer for upcomming amount of steps in 20ms cycle
bool lowstep = false; // trigger for streched step duration
bool hallfound = false; // trigger for hallsensor seen
int hallcount = -1; // counter for hallsensor seen -> -1 at startup
int countback = 0; // counter for revers steps till midposition
bool setup_done = false; // setup (findTableZero) done if true
unsigned long last = 0; // last loop time measurement counter
bool pZero = false; // position zero of turntable found if true
bool measLimit = true; // flag to trigger hallsensor limit finding
// generig definitions for debug
#define DEBUG_TT 1 // set activ if debuging of turntable is needed
//#define DEBUG_TTX 1 // set activ if extended debuging of turntable is needed
#ifdef DEBUG_TTX
unsigned long startime = 0;
unsigned long lastduration = 0;
int countT = 1;
#endif
void setupTable() { // setup function for stepper motor
turntable.setSpeed(stepSpeed); // set stepper duration per step
#ifdef DEBUG_TT
Serial.print("SetupTable -> stepSpeed: ");Serial.println(stepSpeed);
#endif
// counter for errorlevel
int counter = stepsPerTurn*3; // give up after 3 round tripps
do { // find turntable position zero step by step
unsigned long now = millis(); // get timestamp for stepper cycle
unsigned long duration = now - last; // calc duration till last run
if (duration >= 5) { // time is over
if (counter == (stepsPerTurn*2)) { // first run with hall sensor measurment activ
setHallLimit(); // to find secured limit for next run
measLimit = false; // deactivate measurment for second run
}
findTableZero(); // call function to find startposition of turntable
counter--; // decrise errorcounter
unsigned long last = now; // setup time for next cycle
#ifdef DEBUG_TT
Serial.print(", count: ");Serial.print(counter);
Serial.print(", pZero: ");Serial.println(pZero);
#endif
}
} while ((counter > 0) && !pZero ); // till error level reached or position found
setup_done = true;
#ifdef DEBUG_S
Serial.print("SetupTable done: ");Serial.println(pZero);
#endif
}
void findTableZero() { // function to find orientation zero for turntable
if (!read_hall(measLimit)) { // turntable is near position zero
if (hallfound) { // and steps till found known
hallcount++; // proceed with counting
c_wise = true; // prepare for clockwise
stepNow(2); // and step two times per function call
#ifdef DEBUG_TT
Serial.print(", 1 count found: ");
#endif
}
else {
if (hallcount < 0) { // startup -> run more than 360 degree to find position
c_wise = true; // prepare for clockwise
stepNow(2); // and step two times per function call
#ifdef DEBUG_TT
Serial.print(", 2 start inside: ");
#endif
}
else { // first time hall sensor seen
hallfound = true; // set infoflag that hall sensor hase been seen
hallcount = 0; // and start counting steps of area
c_wise = true; // prepare for clockwise
stepNow(2); // and step two times per function call
#ifdef DEBUG_TT
Serial.print(", 3 found first: ");
#endif
}
}
}
else { // turntable is not near position zero
if (hallfound) { // we have past hall sensor area
if (hallcount < 0) { // startup was in hall sensor arera
hallcount = 0; // start the long way round
c_wise = true; // prepare for clockwise
stepNow(2); // and step two times per function call
#ifdef DEBUG_TT
Serial.print(", 4 run outside: ");
#endif
}
else { // we have past hall sensor area right now
countback = (hallcount+1)/2; // half the staps back for midpoint
for (int i = 0; i < countback; i++){
c_wise = false; // prepare for clockwise
stepNow(5); // and step two times per function call
#ifdef DEBUG_TT
Serial.print(", 5 go back: ");Serial.print(countback);Serial.print(" Steps ");
#endif
}
stepOffset = 0; // position zero reached
pZero = true;
LED_Smode[0] = true; // LED set to blue for 3 seconds
}
}
else { // startup is outside hall sensor area
hallcount = 0; // start the long way round
c_wise = true; // prepare for clockwise
stepNow(2); // and step two times per function call
#ifdef DEBUG_TT
Serial.print(", 6 start outside: ");
#endif
}
}
#ifdef DEBUG_TT
Serial.print("ST hallfound: ");Serial.print(hallfound);
Serial.print(", hallcount: ");Serial.print(hallcount);
#endif
} // end function findTableZero
void runTable() { // main turntable function to select mode
#ifdef DEBUG_TT
Serial.println(" ");
#endif
if (!setup_done) { // wait till turntable setup is finished
#ifdef DEBUG_TT
Serial.println("No TT calibration done.");
#endif
return;
}
if (switchOn[0]) { // check if ship is in RotMode
if (switchOn[1]) { // check if turntable should be syncronised
#ifdef DEBUG_TT
Serial.print("rsTable: ");
#endif
LED_Tmode = 4; // set matching LED mode
runPosTable(); // SyncMode is aktiv, calculate turntable direction
if (c_wise) SyncFlashLED(CycleSteps); // coordinate FlashLED sequenze according
else SyncFlashLED(-CycleSteps); // to given turntable position and rotation direction
}
else { // no SyncMode activ, but RotMode
LED_Tmode = 3; // set matching LED mode
#ifdef DEBUG_TT
Serial.print("r_Table: ");
#endif
runRotTable(); // calculate rotation according the given yaw values
if (c_wise) SyncFlashLED(CycleSteps); // coordinate FlashLED sequenze according
else SyncFlashLED(-CycleSteps); // to given turntable position and rotation direction
}
}
else { // RotMode is off
if (switchOn[1]) { // check if turntable should be syncronised
LED_Tmode = 2; // set matching LED mode
#ifdef DEBUG_TT
Serial.print("psTable: ");
#endif
runServoTable(Prop_now); // yes ServoMode is activ, calculate turntable direction
ServoFlashLED(Prop_now); // coordinate FlashLED sequenze according to given prop position
}
else { // no PropMode is activ
LED_Tmode = 1; // set matching LED mode
#ifdef DEBUG_TT
Serial.print("p_Table: ");
#endif
if (Prop_now != 0) { // only if prop value is not zero
runPropTable(Prop_now); // rotation according the proportional part of SP
if (c_wise) PropFlashLED(CycleSteps); // coordinate FlashLED sequenze according
else PropFlashLED(CycleSteps); // to given turntable position and rotation direction
}
}
}
} // end function runTable
void runPropTable(float Speed) { // function to control turntable via RC value (like an ECU)
unsigned int steps2go = Speed*10; // normalize speed request to needed steps in time interval 20ms
#ifdef DEBUG_TT
Serial.print("Speed: ");Serial.print(Speed);
#endif
PrepDeltaStep(steps2go, false); // prepare for the calculated steps in this cycle
// No else -> no movement of turntable requiered
} // end function runPropTable
void runRotTable() { // function to rotate turntable againgst shiprotation
float YawPos; // position wehre stepper must go in degree
float CompRate; // compensation value for high yawrates
int PosCalc;
int RotSteps;
if (abs(yawrate) <= yawrate_idle) { // no rotation, just set yaw position to zero
PosCalc = 0; // transfer into interger steps
#ifdef DEBUG_TT
Serial.print("i ");
#endif
}
else {
YawPos = calcYawPos() / stepAngel; // transform Yaw into steps
CompRate = (yawrate * 0.01) / stepAngel; // calculate lead yaw with half of the normal RC signal loop of 20ms
YawPos = YawPos + CompRate; // compensate yaw
PosCalc = YawPos; // transfer into interger steps
#ifdef DEBUG_TT
Serial.print("r ");
Serial.print(", yaw: ");Serial.print(yaw);
Serial.print(" YawPos: ");Serial.print(YawPos);
#endif
}
PosCalc = PosCalc - stepOffset; // calc the amount of delta steps
#ifdef DEBUG_TT
Serial.print(", stepOffsetStart: ");Serial.print(stepOffset);
Serial.print(", PosCalc: ");Serial.print(PosCalc);
#endif
if (PosCalc >= 0) { // short rotation is clockwise
if (PosCalc > stepsPerHalf) { // clockwise is long way
RotSteps = PosCalc - stepsPerTurn; // set short way with negativ value
}
else {
RotSteps = PosCalc; // clockwise is short
}
}
else { // short rotation is anticlockwise
if (PosCalc < -stepsPerHalf) { // anticlockwise is long way
RotSteps = stepsPerTurn + PosCalc; // set short way with positiv value
}
else {
RotSteps = PosCalc; // anticlockwise ist short
}
}
PrepDeltaStep(RotSteps, true); // prepare for the calculated steps in this cycle
} // end function runRotTable
void runPosTable() { // function to step turntable to the lineare movement orientation of ship
float StepperNow = stepAngel * stepOffset; // calc actuel position off stepper as angle from 0 to 360
float Mo_Pos; // virtual angel of movement direction
float Yaw_Pos; // virtual angel of yaw direction
float DeltaPos; // angle the turntable should point to
float TempDelta; // value for partial results
int DeltaSteps; // amount of needed steps from actual turntable position to movement direction
#ifdef DEBUG_TT
Serial.print("yaw: ");Serial.print(yaw);
Serial.print(", yawrate: ");Serial.print(yawrate);
Serial.print(", stepOffsetStart: ");Serial.print(stepOffset);
Serial.print(", StepperNow: ");Serial.print(StepperNow);
#endif
Mo_Pos = calcOrientPos(); // get movement direction from mo_degree (0 to 360 degree)
Yaw_Pos = calcYawPos(); // get rotation direction from yaw value (0 to 360 degree)
Yaw_Pos = Yaw_Pos + (yawrate * 0.01); // calculate lead yaw with half of the normal RC signal loop of 20ms
#ifdef DEBUG_TT
Serial.print(", Mo_Pos: ");Serial.print(Mo_Pos);
Serial.print(", Yaw_Pos: ");Serial.print(Yaw_Pos);
#endif
DeltaPos = Mo_Pos + Yaw_Pos; // sum up both virtual angles
if (DeltaPos >= 360) {
DeltaPos = DeltaPos - 360; // and trimm it
}
TempDelta = DeltaPos - StepperNow; // calc the delta angle for this cycle
if (TempDelta >= 0) { // short rotation is clockwise
if (TempDelta > 180) { // clockwise is long way
DeltaPos = TempDelta - 360; // set short way with negativ value
}
else {
DeltaPos = TempDelta; // clockwise is short
}
}
else { // short rotation is anticlockwise
if (TempDelta < -180) { // anticlockwise is long way
DeltaPos = 360 + TempDelta; // set short way with positiv value
}
else {
DeltaPos = TempDelta; // anticlockwise ist short
}
}
#ifdef DEBUG_TT
Serial.print(", DeltaPos: ");Serial.print(DeltaPos);
#endif
DeltaSteps = DeltaPos / stepAngel; // calc delta degree into delta steps
PrepDeltaStep(DeltaSteps, false); // prepare for the calculated steps in this cycle
} // end function runPosTable
void runServoTable(float ServoPos) { // function to step turntable via RC value from -180 to 180 degree
int stepPos = 100 * ServoPos; // normalize angle request to needed steps from stepOffset = 0
if (stepPos < 0) { // transform negativ request to high stepoffsets
stepPos = stepsPerTurn + stepPos; // subtract from max steps ammount
}
int stepDelta;
#ifdef DEBUG_TT
Serial.print("stepOffsetStart: ");Serial.print(stepOffset);
Serial.print(", ServoPos: ");Serial.print(ServoPos);
Serial.print(", stepPos: ");Serial.print(stepPos);
#endif
// set deadzone limits and go to stepoffset zero
if ((ServoPos > deadzoneNProp) && (ServoPos < deadzoneProp)) {
if (stepOffset > stepsPerHalf) { // go clockwise
stepDelta = stepsPerTurn - stepOffset; // with positiv values
#ifdef DEBUG_TT
Serial.print(", c ");
#endif
}
else { // go anticlockwise
stepDelta = - stepOffset; // with negativ values
#ifdef DEBUG_TT
Serial.print(", a ");
#endif
}
}
else if (ServoPos > 0) { // check rotation direction
if (stepOffset <= stepsPerHalf) { // stepper ist already on clock side
stepDelta = stepPos - stepOffset; // calc clockwise with positiv values
#ifdef DEBUG_TT
Serial.print(", cc ");
#endif
}
else { // stepper is on the wrong side
// must be clockwise with positiv values
stepDelta = stepPos + stepsPerTurn - stepOffset;
#ifdef DEBUG_TT
Serial.print(", ca ");
#endif
}
}
else { // action is on anticlock side
if (stepOffset >= stepsPerHalf) { // stepper ist already on anticlock side
stepDelta = stepPos - stepOffset; // calc anticlockwise with negativ values
#ifdef DEBUG_TT
Serial.print(", aa ");
#endif
}
else { // stepper is on the wrong side
// must be anticlockwise with negativ values
stepDelta = stepPos - stepsPerTurn - stepOffset;
#ifdef DEBUG_TT
Serial.print(", ac ");
#endif
}
}
PrepDeltaStep(stepDelta, false); // prepare for the calculated steps in this cycle
CycleSteps = 0; // slow function no further steps in this cycle needed
} // end function runServoTable
float calcOrientPos() { // function to transform mo_degree to value 0 to 360
// mo_degree pionts to linear movement direction
float tempPos = mo_degree; // copy mo_degree for calculation
if (mo_degree < 0) { // check if value is negativ
tempPos = 360 + tempPos; // map it to 181 to 360
}
#ifdef DEBUG_TT
Serial.print(", OrientPos: ");Serial.print(tempPos);
#endif
return tempPos; // return value
} // end function CalcOrientPos(
float calcYawPos() { // function to transform yaw to value 0 to 360
float yawPos = yaw; // copy yaw for calculation
if (yaw < 0) { // check if value is negativ
yawPos = 360 + yawPos; // map it to 181 to 360
}
#ifdef DEBUG_TT
//Serial.print(", YawPos: ");Serial.print(yawPos);
#endif
return yawPos; // return value
} // end function CalcOrientPos
bool PrepDeltaStep(int Dsteps, bool led) { // function to transform amount of steps to step duration
float stepDuration; // timeinterval of needed steps
bool maxSpeed = false; // flag set if stepper is to slow
if (abs(Dsteps) > 0) { // check if there is a delta to last cycle
stepDuration = 20/abs(float(Dsteps)); // transfer amount of steps to step duration for 20ms
if (stepDuration < stepDurationMin) { // check if stepper can perform the speed
stepDuration = stepDurationMin; // high delta -> give full speed
maxSpeed = true;
}
stepDurationHigh = stepDuration - 0.25; // buffer step duration in full ms (higer value if in between numbers)
stepDurationLow = stepDuration + 0.25; // buffer step duration in full ms (lower value if in between numbers)
TnextStep = stepDurationHigh + millis(); // set time for next step in 20ms interval
NextStepLow = true; // set toggleflg for second step call in cycle
if (Dsteps > 0) {
CycleSteps = Dsteps; // buffer amount of steps in this cycle
c_wise = true; // buffer step clockwise
stepNow(stepDurationHigh); // step first time in 20ms interval
}
else if (Dsteps < 0) {
CycleSteps = - Dsteps; // buffer amount of steps in this cycle
c_wise = false; // buffer step anticlockwise
stepNow(stepDurationHigh); // step first time in 20ms interval
}
}
else { // no steps needed in this cycle
CycleSteps = 0; // buffer amount of steps in this cycle
stepDurationHigh = stepDurationMax + 1; // buffer step duration in full ms (higer value if in between numbers)
stepDurationLow = stepDurationMax + 1; // buffer step duration in full ms (lower value if in between numbers)
TnextStep = stepDurationHigh + millis(); // set time for next step in 20ms interval
}
// buffer step duration
if (led && maxSpeed) {LED_Smode[1] = true;} // set LED mode if stepper is to slow
else {LED_Smode[1] = false;}
#ifdef DEBUG_TT
Serial.print(", stepDelta: ");Serial.print(Dsteps);
Serial.print(" -> Duration: ");Serial.print(stepDurationHigh);Serial.print("/");Serial.print(stepDurationLow);
Serial.print(" ,c_wise: ");Serial.print(c_wise);
Serial.print(", stepOffsetEnd: ");Serial.println(stepOffset);
Serial.print("Stepsleft: ");Serial.print(CycleSteps);
#endif
return maxSpeed; // return value
} // end function DoDeltaStep
void stepNow(int msDuration) { // function to time step for given duration
#ifdef DEBUG_TTX
if (countT < 2){
startime = millis();
}
#endif
if (msDuration < 2){ // below 2ms is to fast for time measurement -> full speed
if (c_wise) { // check rotation direction
calcOffset(2); // update Offsetcounter
CycleSteps = CycleSteps - 2; // update amount of steps in this cycle
turntable.step(-2); // and step twiche
}
else {
calcOffset(-2); // update Offsetcounter
CycleSteps = CycleSteps - 2; // update amount of steps in this cycle
turntable.step(2); // and step twiche anticlockwise
}
#ifdef DEBUG_TTX
countT++;
countT++;
#endif
}
else { // slower than 2ms per step
#ifdef DEBUG_TTX
countT++;
#endif
if (c_wise) { // check rotation direktion
calcOffset(1); // update Offsetcounter
CycleSteps--; // update amount of steps in this cycle
turntable.step(-1); // and step
}
else {
calcOffset(-1); // update Offsetcounter
CycleSteps--; // update amount of steps in this cycle
turntable.step(1); // and step anticlockwise
}
}
#ifdef DEBUG_TTX
if (countT > stepLimit){
countT = 1;
lastduration = millis() - startime;
Serial.print(", msDuration: ");Serial.print(msDuration);
Serial.print(", Offset: ");Serial.print(stepOffset);
Serial.print(", Duration: ");Serial.println(lastduration);
Serial.print("-> RPM: ");Serial.println(60000/lastduration);
}
#endif
} // end function stepNow
void ReStep() { // function to time multiple steps in 20ms interval
if (CycleSteps > 0) { // wait till multiple steps in cycle is needed
unsigned long Tnow = millis(); // get time
if ( Tnow >= TnextStep) { // check if time til next step hase elapsed
if (CycleSteps == 1) { // check if there is only on step left in 20ms interval
stepNow(3); // last step just do one step, duration > 2ms needed
return; // no mor determination needed this time
}
if (NextStepLow) { // check if is time for high or low duration
TnextStep = stepDurationLow + Tnow; // prepare for next step time as low
stepNow(stepDurationLow); // step next time in 20ms interval
}
else {
TnextStep = stepDurationHigh + Tnow; // prepare for next step time as high
stepNow(stepDurationHigh); // step next time in 20ms interval
}
NextStepLow = !NextStepLow; // toggle flag
#ifdef DEBUG_TT
Serial.print(",");Serial.print(CycleSteps);
#endif
}
}
}
bool calcOffset(int steps) { // function to count nummer of steps done from orientation zero
// returns true if stepper passes orientation
int tempOff = stepOffset + steps; // calculate temporarily steps
bool pass = false; // flag for pass of oriantation point
if (tempOff > stepLimit) { // orientation point passed clockwise
tempOff = tempOff - stepLimit; // reset Offset
pass = true;
}
else if (tempOff < 0) { // orientation point passed anticlockwise
tempOff = stepLimit + tempOff; // reset Offset
pass = true;
}
stepOffset = tempOff; // copy Offset to global value
return pass; // and return orientation point passed flag
} // end function calcOffset
int tablePos(int numseg) { // function to calc turntable position with given number of cyclesegments
// calc segment of actual turntable position (count is clockwise)
float tempseg = (stepOffset * numseg) / stepsPerTurn;
int segment = (int)(tempseg + 0.5); // transfer segment to int and round it
return segment; // return result
} // end function tablePos