I have the IMU input's connected as per the instructable , A Syren 25a connected to GND and pin 13 and a push to make button connected from 5v through a 10k resistor to pin 9 again through a 10k resister to GND.
And I think there something going wrong in the communication between the arduino and Syren but I cant work out where or why.
The reason I think that is the problem is that when I release the deadman button connected to pin 9 the motor just spins up ... when it should stop completely.
Until Im sure the communication between the arduino and Syren is working I cant trust how the machine is responding ... and not having a deadman button makes testing hazardous at best .
I know the arduino and Syren CAN communicate with each other as I've run a different sketch using serial pins 0 and 1 and the motor responds fine so I don't think it's hardware this time.
#include <SoftwareSerial.h>
//Digital pin 13 is serial transmit pin to sabertooth
#define SABER_TX_PIN 13
#define SABER_RX_PIN 12
//set baudrate to match sabertooth dip settings
#define SABER_BAUDRATE 9600
#define SABER_MOTOR1_FULL_FORWARD 255
#define SABER_MOTOR1_FULL_REVERSE 0
#define SABER_ALL_STOP 127
SoftwareSerial SaberSerial (SABER_RX_PIN, SABER_TX_PIN );
void initSabertooth (void) {
pinMode ( SABER_TX_PIN, OUTPUT );
SaberSerial.begin( SABER_BAUDRATE );
SaberSerial.write(127); //kill motors when first switched on
}
float level=0;
float aa = 0.005;
int Steer = 0;
float accraw;
float x_acc;
float accsum;
float x_accdeg;
float gyrosum;
float hiresgyrosum;
float g;
float s;
float t;
float gangleratedeg;
//float gangleratedeg2;
int adc1;
//int adc4;
int adc5;
float gangleraterads;
float ti = 2.2;
//float k1;
//int k2;
//int k3;
int k4;
//float k5;
float overallgain;
float gyroangledt;
float angle;
float anglerads;
float balance_torque;
float softstart;
float cur_speed;
float cycle_time = 0.008;
//float forwardback;
//float leftright;
//float Balance_point;
float balancetrim;
//int balleft;
//int balright;
float a0, a1, a2, a3, a4, a5, a6; //Sav Golay variables. The TOBB bot describes this neat filter for accelerometer called Savitsky Golay filter.
//More on this plus links on my website.
int i;
int j;
int tipstart;
signed char Motor1percent;
//ANALOG INPUTS. Wire up the IMU as in my Instructable and these inputs will be valid.
int accelPin = 4; //pin 4 is analog input pin 4. z-acc to analog pin 4
int gyroPin = 3; //pin 3 is analog balance gyro low-res input pin. Y-rate to analog pin 3
int hiresgyroPin = 0; //hi res balance gyro input is analog pin 0. Y-rate 4.5 to analog pin 0
//The IMU has a low res and a high res output for each gyro. Low res one used by software when tipping over fast and higher res one when tipping gently.
//DIGITAL INPUT
int deadmanbuttonPin = 9; // deadman button is digital input pin 9
//DIGITAL OUTPUT
int oscilloscopePin = 8; //oscilloscope pin is digital pin 8 so you can work out the program loop time (currently about 5.5millisec)
void setup() // run once, when the sketch starts
{
initSabertooth( );
//analogINPUTS
pinMode(accelPin, INPUT);
pinMode(gyroPin, INPUT);
//pinMode(steergyroPin, INPUT);
//pinMode(leftrightPin, INPUT);
// pinMode(forwardbackPin, INPUT);
pinMode(hiresgyroPin, INPUT);
//digital inputs
pinMode(deadmanbuttonPin, INPUT);
//digital outputs
pinMode(oscilloscopePin, OUTPUT);
//Serial.begin(9600); // HARD wired Serial feedback to PC for debugging in Wiring
}
void sample_inputs() {
gyrosum = 0;
// steersum = 0;
hiresgyrosum = 0;
accraw = analogRead(accelPin); //read the accelerometer pin (0-1023)
//Take a set of 7 readings very fast
for (j=0; j<7; j++) {
adc1 = analogRead(gyroPin);
adc5 = analogRead(hiresgyroPin);
gyrosum = (float) gyrosum + adc1; //sum of the 7 readings
hiresgyrosum = (float)hiresgyrosum +adc5; //sum of the 7 readings
}
k4 = digitalRead(deadmanbuttonPin); //this is needed - if you let go the motors both stop for your own safety
overallgain = 0.2;//now fixed in the software NOTE, if your board is too "jumpy" reduce this value a little or vice versa
//ACCELEROMETER READINGS
// Savitsky Golay filter for accelerometer readings. It is better than a simple rolling average which is always out of date.
// SG filter looks at trend of last few readings, projects a curve into the future, then takes mean of whole lot, giving you a more "current" value - Neat!
// Lots of theory on this on net.
a0 = a1;
a1 = a2;
a2 = a3;
a3 = a4;
a4 = a5;
a5 = a6;
a6 = (float) accraw;
accsum = (float) ((-2*a0) + (3*a1) + (6*a2) + (7*a3) + (6*a4) + (3*a5) + (-2*a6))/21;
//accsum isnt really a "sum" (it used to be once),
//now it is the accelerometer value from the rolling SG filter on the 0-1023 scale
digitalWrite(oscilloscopePin, HIGH); //puts out signal to oscilloscope
//NOTE: FINE-TUNE THE VALUE OF 338 BELOW USING METHOD DESCRIBED IN THE IMU TESTER PROCEDURE It sets the balance point
x_accdeg = (float)((accsum - (336 + balancetrim))*0.862); //approx 1.16 steps per degree so divide by 1.16 i.e. multiply by 0.862
// x_accdeg = (float)((accsum - 350) * 0.862); //approx 1.16 steps per degree so divide by 1.16 i.e. multiply by 0.862
if (x_accdeg < -72) x_accdeg = -72; //rejects silly values to stop it going berserk!
if (x_accdeg > 72) x_accdeg = 72;
//Low res gyro rate of tipping reading calculated first
gangleratedeg = (float)(((gyrosum/7) - g)*2.44); //divide by 0.41 for low res balance gyro i.e. multiply by 2.44
if (gangleratedeg < -450) gangleratedeg = -450; //stops crazy values entering rest of the program
if (gangleratedeg > 450) gangleratedeg = 450;
if (gangleratedeg < 100 && gangleratedeg > -100) {
gangleratedeg = (float)(((hiresgyrosum/7) - t)*0.538); //divide by 1.86 i.e. multiply by 0.538
if (gangleratedeg < -110) gangleratedeg = -110;
if (gangleratedeg > 110) gangleratedeg = 110;
}
digitalWrite(oscilloscopePin, LOW); //cuts signal to oscilloscope pin so we have one pulse on scope per cycle of the program so we can work out cycle time.
gyroangledt = (float) ti * cycle_time * gangleratedeg;
gangleraterads = (float) gangleratedeg * 0.017453; //convert to radians - just a scaling issue from history
//angle = (float) ((1-aa) * (angle + gyroangledt)) + (aa * x_accdeg);
angle = (float) ((1-aa) * (angle + gyroangledt)) + (aa * x_accdeg);
anglerads = (float) angle * 0.017453; //converting to radians again a historic scaling issue from past software
//debugging
//Serial.print("Angle = ");
//Serial.println(angle);
balance_torque = (float) (4.5 * anglerads) + (0.5 * gangleraterads);
cur_speed = (float) (cur_speed + (anglerads * 10 * cycle_time)) * 0.999;
level = (float)balance_torque * overallgain;
}
void set_motor() {
unsigned char cSpeedVal_Motor1 = 0;
level = level * 200; //changes it to a scale of about -100 to +100
Steer = 0;
//All steering is currently removed from this program.
Motor1percent = (signed char) level + Steer;
if (Motor1percent > 100) Motor1percent = 100;
if (Motor1percent < -100) Motor1percent = -100;
//if not pressing deadman button on hand controller - cut everything
if (k4 < 1) {
level = 0;
Steer = 0;
Motor1percent = 0;
}
cSpeedVal_Motor1 = map (Motor1percent,
-100,
100,
SABER_MOTOR1_FULL_REVERSE,
SABER_MOTOR1_FULL_FORWARD);
SaberSerial.write (cSpeedVal_Motor1);
delay(4); //4ms delay
}
void loop () {
tipstart = 0;
overallgain = 0;
cur_speed = 0;
level = 0;
Steer = 0;
balancetrim = 0;
for (i=0; i<200; i++) {
sample_inputs();
}
g = (float) gyrosum/7; //gyro balance value when stationary i.e. 1.35V
// s = (float) steersum/7; //steer gyro value when stationary i.e. about 1.32V
t = (float) hiresgyrosum/7; //hiresgyro balance output when stationary i.e. about 1.38V
//tiltstart routine now comes in.
while (tipstart < 5) {
for (i=0; i<10; i++) {
sample_inputs();
}
//x_accdeg is tilt angle from accelerometer in degrees
if (x_accdeg < -2 || x_accdeg > 2) {
tipstart = 0;
overallgain = 0;
cur_speed = 0;
level = 0;
Steer = 0;
balancetrim = 0;
}
else {
tipstart = 5;
}
}
overallgain = 0.3; //THIS JUST SETS IT THE FIRST TIME
angle = 0;
cur_speed = 0;
balancetrim = 0;
//end of tiltstart code.
sample_inputs();
set_motor();
}
}
I soon as release the deadman button the motorpercent drops to zero .... everything else seems to be fine the Gro and Accel readouts change as expected as does the motorpercent.
fish_man:
and a push to make button connected from 5v through a 10k resistor to pin 9 again through a 10k resister to GND.
Am I doing that right one terminal of the switch is connected trough a 10K resistor to 5v and the other is connected to pin9 AND through another 10k resistor to ground?
But my gut feeling is that this is not the problem ... like I say the button appears to work fine when I output to the serial window and it's not simply a case of the button not working at all
It's that when the button is released the motor spins up and doesn't just kill the motor like it should.
You mention using internal pull-up resistor's I heared this mentioned before what would it entail?
it would make the wiring easier is I could ditch these external resistors.
I cant believe its taken me this long to test this but it appears that the deadman button doesnt work at all.
It appears to be working in the serial window as described above but.
Normal operation would be ( Ie the way I WANT it to work) - turn on arduino with the machine tilted -- allow time for arduino to get readings from IMU - press in button - tilt machine upright --- and it starts to work, Until the button is released (see below)
What it actually does , The tipstart works fine ... ie nothing happens until the machine is tilted upright ... but
If the button is pressed it act's "normally" (I've not got it to balance yet but that's another challenge) The motor spins, speeds up, slows down, changes direction etc .
If the button isn't pressed then the motor just spins in one direction at one speed as best I can tell ... (it's hard to test a machine with a 20" wheel spinning uncontrollably which causes damage if it hit's anything).
it's hard to test a machine with a 20" wheel spinning uncontrollably which causes damage if it hit's anything
It's not hard to write a test sketch that simply polls the switch state and prints to the serial monitor "Pressed" or "Released".
Most likely, the switch is not wired correctly (so the pin floats) or the switch is bouncing, or the logic for pressed and released is inverted.
LadyAda's site has a good tutorial on pullup and pulldown resistors, and how to use the internal one. The top picture on the link I posted shows a pull-down resistor. The bottom picture shows a pullup resistor. The external pullup resistor can be removed, and replaced with a call to digitalWrite(switchPin, HIGH); after the pinMode() call for the pin, to turn on the pullup resistor.
If the button isn't pressed then the motor just spins in one direction at one speed as best I can tell ... (it's hard to test a machine with a 20" wheel spinning uncontrollably which causes damage if it hit's anything).
For simulating motor operation, you could connect a 2 LEDs to 2 different pins and have one light up when "reverse" is called, and the other light when "forward" is called. When you have predictable results you can return to using the actual motor.
For now I think I'm just going to comment out the dead-mans switch and concentrate on balancing.
the deadman switch is actually more dangerous as it is, then it would be removed.
if I cant get it to balance then the deadman switch becomes obsolete anyway.
If anyone does spot a obvious answer to the problem in the code then please let me know ... or indeed any problems with the code full stop , as I'm sure there are many ... it was originally cobbled together by someone else and Then I hacked it about a fair bit .. so I'm pretty sure it's a right mess , It could probably do with someone scrapping it and starting over ... but I've not got the skills for that.
the deadman switch is actually more dangerous as it is, then it would be removed.
Bad idea. There is something fundamentally wrong with your wiring if you can't read a simple switch properly.
I don't see how it can be the switch or wiring .. I will happily test it the next time I am at the machine ... but the switch is causing something to happen when pressed and something else to happen when not pressed ,surely that means it is being read ok, but something is wrong in the code ?
When I output to the serial window the switch is doing exactly as I want it to do IE changing the motorpercent to zero
k4 = digitalRead(deadmanbuttonPin); //this is needed - if you let go the motors both stop for your own safety
but then you do this with the result
//if not pressing deadman button on hand controller - cut everything
if (k4 < 1) {
level = 0;
Steer = 0;
Motor1percent = 0;
should it not be :-
//if not pressing deadman button on hand controller - cut everything
if (k4 = LOW) { (// or HIGH if you are using the internal pullup)
level = 0;
Steer = 0;
Motor1percent = 0;
Common beginners mistake, = is an assignment operator == is a equality operator.
So
should it not be :-
//if not pressing deadman button on hand controller - cut everything
if (k4 = LOW) { // or HIGH if you are using the internal pullup
level = 0;
Steer = 0;
Motor1percent = 0;
Mark
Should be:
should it not be :-
//if not pressing deadman button on hand controller - cut everything
if (k4 == LOW) { // or HIGH if you are using the internal pullup
level = 0;
Steer = 0;
Motor1percent = 0;
Mark
Or to get a little C fancy (and as an exercise for the student to look up) you could do:
should it not be :-
//if not pressing deadman button on hand controller - cut everything
if (!k4) { // or if (k4) if you are using the internal pullup
level = 0;
Steer = 0;
Motor1percent = 0;
Mark
Thanks for that . I have a couple of things to try now, The original programmer has offered up a different solution for me to try as well.
Hopefully I should be able to add it back in.
Yes despite advice I commented it out and have managed to get it to balance , Link to video is here Franken-Wheelie Improved - YouTube for anyone who is interested.
As I was testing it earlier it was doing exactly what it was in the video ... and then suddenly .. it was working perfectly, smooth, no noise, no juddering, no jerking. I even jumped on and had a quick try .... it was perfect !!
Anyway after some "diagnostic's" ( sounds weird to diagnose why something is working perfectly doesn't it) I found the battery was low .. and sure enough as soon as I connected up another battery I wasn't able to replicate it working well.
So I figure reducing the gains should solve it ... I've tried it before but I figure what the hell try again I may have made a mistake last time .. so I reduce all the gains WAAAAY down and it makes no difference to the unicycle except making it feel mushy and unresponsive , The shuddering and jerking is still there.