I didn’t figure out what your code should do. So you are right and I understand why you mention 10.
1 Like
It really should work, all it is doing is doing the same thing as when the counter hits the limit. Can you post the whole ino?
Just attach as a text file
Here is what I have been using. If you see anything wrong please let me know. It would be greatly appreciated. My self taught hours for c++ could probably be counted on my hands. I am not sure how to attach files on here yet…
// MODIFIED - V4 USB AGOPENGPS
// This Script has been modified by Slimfarmer on 3-25-2020
// Modified to activate a hydraulic valve block that uses a shuttle shut off solenoid on Pin 2 when auto guidance is engaged.
// Modified to use a 0-5 volt pressure transducer sensor on ADS 1115 Aux2 input to turn off auto steer when steering wheel is turned.
//
// MODIFIED SETTINGS
int SteeringZerostartingsetpoint = 2100; // Use this to easily change your Zero point if the + or - 127 in the program is not enough
//
// Pressure Transducer sensor for manual steering wheel movement
long SteeringWheelPressureReading = 0;
byte SteeringWheelPressureReadingLimit = 2000;
//
//----------------------------------------------------------------------------
//////////////////// *********** Motor drive connections **************888
//Connect ground only for cytron, Connect Ground and +5v for IBT2
//Dir1 for Cytron Dir, Both L and R enable for IBT2
#define DIR1_RL_ENABLE 4 //PD4
//PWM1 for Cytron PWM, Left PWM for IBT2
#define PWM1_LPWM 3 //PD3
//Not Connected for Cytron, Right PWM for IBT2
#define PWM2_RPWM 9 //D9
//int0 Steering Wheel Encoder - turns Autosteer off
#define encAPin 2 //PD2
//--------------------------- Switch Input Pins ------------------------
#define STEERSW_PIN 6 //PD6
#define WORKSW_PIN 7 //PD7
#define REMOTE_PIN 8 //PB0
#include <Wire.h>
#include <EEPROM.h>
#include "zADS1015.h"
Adafruit_ADS1115 ads; // Use this for the 16-bit version ADS1115
//GY-45 (1C)
//installed Sparkfun, Adafruit MMA8451 (1D)
#include "MMA8452_AOG.h" //MMA inclinometer
MMA8452 MMA1D(0x1D);
MMA8452 MMA1C(0x1C);
uint16_t x_ , y_ , z_;
#include "BNO055_AOG.h" // BNO055 IMU
#define A 0X28 //I2C address selection pin LOW
#define B 0x29 // HIGH
#define RAD2GRAD 57.2957795
BNO055 IMU(A); // create an instance
#define EEP_Ident 0xEDFB
//loop time variables in microseconds
const unsigned int LOOP_TIME = 100;
unsigned long lastTime = LOOP_TIME;
unsigned long currentTime = LOOP_TIME;
byte watchdogTimer = 20;
byte serialResetTimer = 100; //if serial buffer is getting full, empty it
//Kalman variables
float rollK = 0, Pc = 0.0, G = 0.0, P = 1.0, Xp = 0.0, Zp = 0.0;
float XeRoll = 0;
const float varRoll = 0.1; // variance,
const float varProcess = 0.0001; //smaller is more filtering
//inclinometer variables
int roll = 0;
//Program flow
bool isDataFound = false, isSettingFound = false, isMachineFound=false, isAogSettingsFound = false;
bool MMAinitialized = false;
int header = 0, tempHeader = 0, temp, EEread = 0;
byte relay = 0, relayHi = 0, uTurn = 0;
byte remoteSwitch = 0, workSwitch = 0, steerSwitch = 1, switchByte = 0;
float distanceFromLine = 0, gpsSpeed = 0;
//steering variables
float steerAngleActual = 0;
float steerAngleSetPoint = 0; //the desired angle from AgOpen
long steeringPosition = 0; //from steering sensor
float steerAngleError = 0; //setpoint - actual
//pwm variables
int pwmDrive = 0, drive = 0, pwmDisplay = 0;
float pValue = 0;
//Steer switch button ***********************************************************************************************************
byte currentState = 1;
byte reading;
byte previous = 0;
byte test = 0;
volatile int pulseCount = 0; // Steering Wheel Encoder
volatile bool encEnable = false; //debounce flag
//Variables for settings
struct Storage {
float Ko = 0.0f; //overall gain
float Kp = 0.0f; //proportional gain
float Ki = 0.0f; //integral gain
float Kd = 0.0f; //derivative gain
float steeringPositionZero = 1660.0;
byte minPWMValue=10;
int maxIntegralValue=20;//max PWM value for integral PID component
float steerSensorCounts=30;
}; Storage steerSettings; //27 bytes
//Variables for settings - 0 is false
struct Setup {
byte InvertWAS = 0;
byte InvertRoll = 0;
byte MotorDriveDirection = 0;
byte SingleInputWAS = 1;
byte CytronDriver = 1;
byte SteerSwitch = 1;
byte UseMMA_X_Axis = 0;
byte ShaftEncoder = 0;
byte BNOInstalled = 0;
byte InclinometerInstalled = 0; // set to 0 for none
// set to 1 if DOGS2 Inclinometer is installed and connected to ADS pin A2
// set to 2 if MMA8452 installed GY-45 (1C)
// set to 3 if MMA8452 installed Sparkfun, Adafruit MMA8451 (1D)
byte MaxSpeed = 20;
byte MinSpeed = 15;
byte PulseCountMax = 5;
byte AckermanFix = 100; //sent as percent
}; Setup aogSettings; //11 bytes
//reset function
void(* resetFunc) (void) = 0;
void setup()
{
//keep pulled high and drag low to activate, noise free safe
pinMode(encAPin, INPUT_PULLUP);
pinMode(WORKSW_PIN, INPUT_PULLUP);
pinMode(STEERSW_PIN, INPUT_PULLUP);
pinMode(REMOTE_PIN, INPUT_PULLUP);
pinMode(DIR1_RL_ENABLE, OUTPUT);
//----------------------------------------------------------------------------------------------
// modified by Slimfarmer on 3-25-2020 <-------------------------------------------------------------------------------------------- MODIFIED
pinMode(2, OUTPUT); // autoguidance shuttle solenoid on pin2
//===============================================================================================
//set up communication
Wire.begin();
Serial.begin(38400);
EEPROM.get(0, EEread); // read identifier
if (EEread != EEP_Ident) // check on first start and write EEPROM
{
EEPROM.put(0, EEP_Ident);
EEPROM.put(2, 1660);
EEPROM.put(10, steerSettings);
EEPROM.put(40, aogSettings);
}
else
{
EEPROM.get(2, EEread); // read SteerPosZero
EEPROM.get(10, steerSettings); // read the Settings
EEPROM.get(40, aogSettings);
}
// BNO055 init
if (aogSettings.BNOInstalled)
{
IMU.init();
IMU.setExtCrystalUse(true); //use external 32K crystal
}
//interrupt pin
attachInterrupt(digitalPinToInterrupt(encAPin), EncoderISR, FALLING);// Hardware IRQ 0
if (aogSettings.InclinometerInstalled == 2 )
{
// MMA8452 (1) Inclinometer
MMAinitialized = MMA1C.init();
if (MMAinitialized)
{
MMA1C.setDataRate(MMA_800hz);
MMA1C.setRange(MMA_RANGE_8G);
MMA1C.setHighPassFilter(false);
}
//else Serial.println("MMA init fails!!");
}
else if (aogSettings.InclinometerInstalled == 3 )
{
// MMA8452 (1) Inclinometer
MMAinitialized = MMA1D.init();
if (MMAinitialized)
{
MMA1D.setDataRate(MMA_800hz);
MMA1D.setRange(MMA_RANGE_8G);
MMA1D.setHighPassFilter(false);
}
//else Serial.println("MMA init fails!!");
}
}// End of Setup
void loop()
{
// Loop triggers every 100 msec and sends back gyro heading, and roll, steer angle etc
currentTime = millis();
if (currentTime - lastTime >= LOOP_TIME)
{
lastTime = currentTime;
//reset debounce
encEnable = true;
//If connection lost to AgOpenGPS, the watchdog will count up and turn off steering
if (watchdogTimer++ > 250) watchdogTimer = 12;
//clean out serial buffer to prevent buffer overflow
if (serialResetTimer++ > 20)
{
while (Serial.available() > 0) char t = Serial.read();
serialResetTimer = 0;
}
if (aogSettings.BNOInstalled)
IMU.readIMU();
if (aogSettings.InclinometerInstalled ==1)
{
//DOGS2 inclinometer
rollK = ((ads.readADC_SingleEnded(2))); // 24,000 to 2700
rollK = (rollK - 13300)*0.0357;
}
// MMA8452 Inclinometer (1C)
else if (aogSettings.InclinometerInstalled == 2)
{
if (MMAinitialized)
{
MMA1C.getRawData(&x_, &y_, &z_);
if (aogSettings.UseMMA_X_Axis)
roll= x_; //Conversion uint to int
else roll = y_;
if (roll > 4200) roll = 4200;
if (roll < -4200) roll = -4200;
rollK = map(roll,-4200,4200,-960,960); //16 counts per degree (good for 0 - +/-30 degrees)
}
}
// MMA8452 Inclinometer (1D)
else if (aogSettings.InclinometerInstalled == 3)
{
if (MMAinitialized)
{
MMA1D.getRawData(&x_, &y_, &z_);
if (aogSettings.UseMMA_X_Axis)
roll= x_; //Conversion uint to int
else roll = y_;
if (roll > 4200) roll = 4200;
if (roll < -4200) roll = -4200;
rollK = map(roll,-4200,4200,-960,960); //16 counts per degree (good for 0 - +/-30 degrees)
}
}
//if not positive when rolling to the right
if (aogSettings.InvertRoll)
rollK *= -1.0;
//Kalman filter
Pc = P + varProcess;
G = Pc / (Pc + varRoll);
P = (1 - G) * Pc;
Xp = XeRoll;
Zp = Xp;
XeRoll = G * (rollK - Zp) + Xp;
//read all the switches
workSwitch = digitalRead(WORKSW_PIN); // read work switch
if (aogSettings.SteerSwitch == 1) //steer switch on - off
{
steerSwitch = digitalRead(STEERSW_PIN); //read auto steer enable switch open = 0n closed = Off
}
else //steer Button momentary
{
reading = digitalRead(STEERSW_PIN);
if (reading == LOW && previous == HIGH)
{
test++;
if (currentState == 1)
{
currentState = 0;
steerSwitch = 0;
}
else
{
currentState = 1;
steerSwitch = 1;
}
}
previous = reading;
}
if (aogSettings.ShaftEncoder && pulseCount >= aogSettings.PulseCountMax )
{
steerSwitch = 1; // from Steeringwheel encoder
currentState = 1;
previous = HIGH;
}
remoteSwitch = digitalRead(REMOTE_PIN); //read auto steer enable switch open = 0n closed = Off
switchByte = 0;
switchByte |= (remoteSwitch << 2); //put remote in bit 2
switchByte |= (steerSwitch << 1); //put steerswitch status in bit 1 position
switchByte |= workSwitch;
/*
#if Relay_Type == 1
SetRelays(); //turn on off section relays
#elif Relay_Type == 2
SetuTurnRelays(); //turn on off uTurn relays
#endif
*/
//--------------------------------------------------------------------------------------------------------------------
//Modified by Slimfarmer on 3-25-2020 <----------------------------------------------------------------------------------------- MODIFIED
//To add 5 volt pressure transducer capability vs a rotory encoder
if (aogSettings.ShaftEncoder)
{
SteeringWheelPressureReading = ads.readADC_SingleEnded(3);
SteeringWheelPressureReading = (SteeringWheelPressureReading * 0.1875F);
if (SteeringWheelPressureReading >= SteeringWheelPressureReadingLimit)
{
steerSwitch = 1; // from Steeringwheel pressure transducer sensor
currentState = 1;
previous = HIGH;
}
}
//--------------------------------------------------------------------------------------------------------------------
//get steering position
if (aogSettings.SingleInputWAS) //Single Input ADS
{
steeringPosition = ads.readADC_SingleEnded(0); //ADS1115 Single Mode
steeringPosition = (steeringPosition >> 3); //bit shift by 3 0 to 3320 is 0 to 5v
}
else //ADS1115 Differential Mode
{
steeringPosition = ads.readADC_Differential_0_1(); //ADS1115 Differential Mode
steeringPosition = (steeringPosition >> 3); //bit shift by 3 0 to 3320 is 0 to 5v
}
//DETERMINE ACTUAL STEERING POSITION
steeringPosition = (steeringPosition - steerSettings.steeringPositionZero); //read the steering position sensor
//convert position to steer angle. 32 counts per degree of steer pot position in my case
// ***** make sure that negative steer angle makes a left turn and positive value is a right turn *****
if (aogSettings.InvertWAS)
steerAngleActual = (float)(steeringPosition) / -steerSettings.steerSensorCounts;
else
steerAngleActual = (float)(steeringPosition) / steerSettings.steerSensorCounts;
//Ackerman fix
if (steerAngleActual < 0) steerAngleActual = (steerAngleActual * aogSettings.AckermanFix)/100;
if (watchdogTimer < 10)
{
if (!aogSettings.CytronDriver)
digitalWrite(DIR1_RL_ENABLE, 1); //Enable H Bridge for IBT2
steerAngleError = steerAngleActual - steerAngleSetPoint; //calculate the steering error
calcSteeringPID(); //do the pid
motorDrive(); //out to motors the pwm value
//---------------------------------------------------------------------------------------------- <--------------------------- MODIFIED
// modified by Slimfarmer on 3-25-2020
digitalWrite(2,LOW); //Activate autoguidance relay
//===============================================================================================
}
else
{
//we've lost the comm to AgOpenGPS, or just stop request
if (!aogSettings.CytronDriver)
digitalWrite(DIR1_RL_ENABLE, 0); //Disable H Bridge for IBT2
pwmDrive = 0; //turn off steering motor
motorDrive(); //out to motors the pwm value
pulseCount=0;
//---------------------------------------------------------------------------------------------- <-----------------------------MODIFIED
// modified by Slimfarmer on 3-25-2020
digitalWrite(2,HIGH); //Deactivate autoguidance relay
//===============================================================================================
}
//Serial Send to agopenGPS **** you must send 10 numbers ****
Serial.print("127,253,");
Serial.print((int)(steerAngleActual * 100)); //The actual steering angle in degrees
Serial.print(",");
Serial.print((int)(steerAngleSetPoint * 100)); //the setpoint originally sent
Serial.print(",");
// ******* if there is no gyro installed send 9999
if (aogSettings.BNOInstalled)
Serial.print((int)IMU.euler.head); //heading in degrees * 16 from BNO
else
Serial.print(9999); //No IMU installed
Serial.print(",");
//******* if no roll is installed, send 9999
if (aogSettings.InclinometerInstalled)
Serial.print((int)XeRoll); //roll in degrees * 16
else
Serial.print(9999); //no Inclinometer installed
Serial.print(",");
//the status of switch inputs
Serial.print(switchByte); //steering switch status
Serial.println(",,,");
Serial.flush(); // flush out buffer
} //end of timed loop
//This runs continuously, not timed //// Serial Receive Data/Settings /////////////////
delay (10);
if (Serial.available() > 0 && !isDataFound && !isSettingFound && !isMachineFound && !isAogSettingsFound)
{
int temp = Serial.read();
header = tempHeader << 8 | temp; //high,low bytes to make int
tempHeader = temp; //save for next time
if (header == 32766) isDataFound = true; //Do we have a match?
if (header == 32764) isSettingFound = true; //Do we have a match?
if (header == 32762) isMachineFound = true; //Do we have a match?
if (header == 32763) isAogSettingsFound = true; //Do we have a match?
}
//Data Header has been found, so the next 8 bytes are the data
if (Serial.available() > 7 && isDataFound)
{
//was section control lo byte
Serial.read();
isDataFound = false;
gpsSpeed = Serial.read() * 0.25; //actual speed times 4, single byte
//distance from the guidance line in mm
distanceFromLine = (float)(Serial.read() << 8 | Serial.read()); //high,low bytes
//set point steer angle * 10 is sent
steerAngleSetPoint = ((float)(Serial.read() << 8 | Serial.read()))*0.01; //high low bytes
//auto Steer is off if 32020,Speed is too slow, motor pos or footswitch open
if (distanceFromLine == 32020 | gpsSpeed < aogSettings.MinSpeed | gpsSpeed > aogSettings.MaxSpeed)
{
watchdogTimer = 12; //turn off steering motor
}
else //valid conditions to turn on autosteer
{
watchdogTimer = 0; //reset watchdog
serialResetTimer = 0; //if serial buffer is getting full, empty it
}
Serial.read();
Serial.read();
}
//Machine Header has been found, so the next 8 bytes are the data
if (Serial.available() > 7 && isMachineFound)
{
isMachineFound = false;
relayHi = Serial.read();
relay = Serial.read();
gpsSpeed = Serial.read() * 0.25; //actual speed times 4, single byte
uTurn = Serial.read();
Serial.read();
Serial.read();
Serial.read();
Serial.read();
}
//ArdSettings has been found, so the next 8 bytes are the data
if (Serial.available() > 7 && isAogSettingsFound)
{
isAogSettingsFound = false;
byte sett = Serial.read(); //setting0
if (bitRead(sett,0)) aogSettings.InvertWAS = 1; else aogSettings.InvertWAS = 0;
if (bitRead(sett,1)) aogSettings.InvertRoll = 1; else aogSettings.InvertRoll = 0;
if (bitRead(sett,2)) aogSettings.MotorDriveDirection = 1; else aogSettings.MotorDriveDirection = 0;
if (bitRead(sett,3)) aogSettings.SingleInputWAS = 1; else aogSettings.SingleInputWAS = 0;
if (bitRead(sett,4)) aogSettings.CytronDriver = 1; else aogSettings.CytronDriver = 0;
if (bitRead(sett,5)) aogSettings.SteerSwitch = 1; else aogSettings.SteerSwitch = 0;
if (bitRead(sett,6)) aogSettings.UseMMA_X_Axis = 1; else aogSettings.UseMMA_X_Axis = 0;
if (bitRead(sett,7)) aogSettings.ShaftEncoder = 1; else aogSettings.ShaftEncoder = 0;
sett = Serial.read(); //setting1
if (bitRead(sett,0)) aogSettings.BNOInstalled = 1; else aogSettings.BNOInstalled = 0;
aogSettings.MaxSpeed = Serial.read(); //actual speed
aogSettings.MinSpeed = Serial.read();
byte inc = Serial.read();
aogSettings.InclinometerInstalled = inc & 192;
aogSettings.InclinometerInstalled = aogSettings.InclinometerInstalled >> 6;
aogSettings.PulseCountMax = inc & 63;
aogSettings.AckermanFix = Serial.read();
Serial.read();
EEPROM.put(40, aogSettings);
resetFunc();
}
//Settings Header has been found, 8 bytes are the settings
if (Serial.available() > 7 && isSettingFound)
{
isSettingFound = false; //reset the flag
//change the factors as required for your own PID values
steerSettings.Kp = (float)Serial.read() * 1.0; // read Kp from AgOpenGPS
steerSettings.Ki = (float)Serial.read() * 0.001; // read Ki from AgOpenGPS
steerSettings.Kd = (float)Serial.read() * 1.0; // read Kd from AgOpenGPS
steerSettings.Ko = (float)Serial.read() * 0.1; // read Ko from AgOpenGPS
steerSettings.steeringPositionZero = SteeringZerostartingsetpoint + Serial.read(); //read steering zero offset // <-----------Modified
steerSettings.minPWMValue = Serial.read(); //read the minimum amount of PWM for instant on
steerSettings.maxIntegralValue = Serial.read()*0.1; //
steerSettings.steerSensorCounts = Serial.read(); //sent as 10 times the setting displayed in AOG
EEPROM.put(10, steerSettings);
}
} // end of main loop
//ISR Steering Wheel Encoder
void EncoderISR()
{
if (encEnable)
{
pulseCount++;
encEnable = false;
}
}
Is your steer switch set to button?
Also, why do you multiply the pressure reading by 0.1875 ? Best to just use the raw numbers like 600 and 2000 or whatever, because i see your PressureReadingLimit is 2000. no need to multiply it.
Also, you are making your pressureLimit a byte - from 0-255. not what you want. Make it an unsigned int or long if it goes above 64,000.
byte SteeringWheelPressureReadingLimit = 2000;
Does this make sense?
I used 0.1875 because the test program I found to read the ads1115 aux3 used it. In your ino. file the WAS bit shifts by three. What exactly does that mean?
//get steering position
if (aogSettings.SingleInputWAS) //Single Input ADS
{
steeringPosition = ads.readADC_SingleEnded(0); //ADS1115 Single Mode
steeringPosition = (steeringPosition >> 3); //bit shift by 3 0 to 3320 is 0 to 5v
I have button selected and sent that to the arduino.
I will try using raw data and change the byte to a int since it doesn’t need decimal places like a long would give.
The >> is called a bit shift operator. 16 >> 1 would result in 8, 16 >>2 would be 4. Its like really efficient division where the bits are shifted over either left or right.
So in the ADS code for WAS, value >> 3 is the same as value/8.
Remember int goes from -32768 to +32767 whereas uint goes from 0 to 65535.
Sounds like I have more google searching for tonight to learn about what you just said haha.
cheers 
ITs a learning process for sure - it never ends
I guess the real question is if I go with raw data on aux 3 of the ads1115 with 0 to 5 volts coming in will it be 0 to 3320 still? That’s what I am after with the 2000 I had.
if you do the value >> 3 like the WAS, then it should be around there. 15 bits is 32,000 divided by 4 is around 4000 full scale
I’ll see what this does tomorrow.
Thanks for the quick replies!
//--------------------------------------------------------------------------------------------------------------------
//Modified by Slimfarmer on 3-25-2020 <----------------------------------------------------------------------------------------- MODIFIED
//To add 5 volt pressure transducer capability vs a rotory encoder
if (aogSettings.ShaftEncoder)
{
SteeringWheelPressureReading = ads.readADC_SingleEnded(3);
SteeringWheelPressureReading = (SteeringWheelPressureReading >> 3); //bit shift by 3 0 to 3320 is 0 to 5v
if (SteeringWheelPressureReading >= SteeringWheelPressureReadingLimit)
{
steerSwitch = 1; // from Steeringwheel pressure transducer sensor
currentState = 1;
previous = HIGH;
}
}
//--------------------------------------------------------------------------------------------------------------------
You might have to play around with your limit value to match what is coming out for a reading - but you should be much closer now.
After watching a youtube video on bit shifting it looks like bits can get lost off the end if you don’t use decimals. Is that why you have to use an unsigned long to prevent that issue?
I think i just used unsigned long but could have used an uint as well. Yes, the bits just fall off the end. All you are “losing” is whether the real answer is 2345.5678 or just 2345.
2345 is good enough resolution
1 Like
I had to lower my SteeringWheelPressureReadingLimit value down to 1300 and now it works beautifully. The button works now to!
Thanks again for the help!
2 Likes
Awesome blossom
I have a similar setup with a 5volt pressure transducer on valve, is this code working with your setup now? How is it wired into your PCB board. Thanks
Yes, I have it working. I tweaked the trigger amount a little lower to make it work better. I have it coming into the ads1115 marked aux 2 on the side of the PCBV2. In the code it has to be ads.readADC_SingleEnded(3) however since that’s where it actually ends up on the ads1115.
This is the INO I currently use.
Autosteer_USB_for_V4_–_Modified_By_Slimfarmer_on_3-25-2020.ino (20.4 KB)
1 Like
I modified your code to this to be able to adjust the sensibility of the steering disengagement by the pressure transducer with the encoder setting in Aog is this correct
//modif le 22/03/21 ====================================================
if (aogSettings.ShaftEncoder)
{
SteeringWheelPressureReading = ads.readADC_SingleEnded(3);
SteeringWheelPressureReading = (SteeringWheelPressureReading >> 3); //bit shift by 3 0 to 3320 is 0 to 5v
if ((pulseCount >= aogSettings.PulseCountMax)||(SteeringWheelPressureReading >=(aogSettings.PulseCountMax*100)))
{
steerSwitch = 1;
currentState = 1;
previous = HIGH;
}
}
2 Likes
looks promising. When version 4.6 comes out ill have to try it out. 1300 or 13 with the conversion in your code seemed to be a good amount for my setup. I look forward to trying it.
Cheers