Go Down

Topic: Balancing unicycle help (Read 2056 times) previous topic - next topic

fish_man

Feb 13, 2012, 03:23 pm Last Edit: Feb 13, 2012, 03:37 pm by fish_man Reason: 1
Hi , I'm working on a balancing unicycle based on someone else's code and I've hit a dead end.

The code I'm using is  Based on this instructable http://www.instructables.com/id/Easy-build-self-balancing-skateboardrobotsegway-/.

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.


fish_man

The code because it wouldnt fit in one post.

Code: [Select]
#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();
 
          }
         
         
 }

fish_man

I should mention that when I run the sketch with serial.print commands that I had to edit out to get it to fit

Code: [Select]
   Serial.print("motorpercent:");
    Serial.print(motorpercent);
    Serial.print("   Angle degrees:");
    Serial.print(angle);
    Serial.print("     Cur_speed:");
    Serial.print(cur_speed);
    Serial.print("   level:");
    Serial.println(level);
   


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.

PaulS

You are not enabling the internal pullup resistor for the deadman switch. So, this means you have an external one, right?

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?

PaulS


fish_man

I will change that thanks.

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.



fish_man

I've tried that no difference.

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).


PaulS

Quote
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.

cyclegadget

Quote
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.
Good links: Eagle tutorial= http://www.youtube.com/playlist?list=PLDE1858BD83D19C70
General Arduion tutorials = http://tronixstuff.wordpress.com
http://www.gammon.com.au/forum/bbshowpost.php?bbtopic_id=123

fish_man

Thanks for all the help,

For now I think I'm just going to comment out the dead-mans switch and concentrate on balancing.

1. the deadman switch is actually more dangerous as it is, then it would be removed.
2. 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.

I'm going to have to deal with it eventually.

PaulS

Quote
1. 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.


fish_man


Quote
1. 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

markB

Guys
Correct me if I'm wrong but when you digitalread the pin, you get a HIGH or LOW (http://arduino.cc/en/Reference/DigitalRead).

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

[font=Verdana] //if not pressing deadman button on hand controller - cut everything
   if (k4 < 1) {
    level = 0;
    Steer = 0;
    Motor1percent = 0;[/font]


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

retrolefty

#14
Feb 23, 2012, 09:23 am Last Edit: Feb 23, 2012, 09:30 am by retrolefty Reason: 1
Common beginners mistake, = is an assignment operator  == is a equality operator.

So

Code: [Select]
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:

Code: [Select]
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:

Code: [Select]
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


Lefty

Go Up