Quadcopter Control Software

lucidshift · 2011-03-03 12:27

Hello,

I am an student going for an electronics major currently working on a quadcopter. The physical construction and electrical wiring is done and I am in the process of writing code. I have some experience working with command prompts and the Picaxe microcontroller, but this is the first major software endeavor I've undertaken. Working with Spin on the Propeller wasn't nearly as easy or fun as I would have liked it to have been.

That being said, I was hoping I could get some feedback and suggestions on the code I have written so far. It works some of the time to say the least. I'm using the Gadget Gangster Usb Propeller board. The input comes from the receiver module of a Vex. In theory the code takes the output value from the receiver module and puts it into a variable representing the magnitude of a vector along the X, Y, and Z axis'. Right now it simply takes those values and performs some basic operations that represent up/down, left/right movements and such. I plan to add a calculus based stabilizing algorithm I have worked out that will take the vector values along with input from a 3-axis accelerometer and a gyroscope out of a Wii MotionPlus to stabilize the quadcopter. I've looked at the output signals from the receiver module and the Propeller board on an oscilloscope and they look fine, so at this point it's safe to say any problem is likely to be software related.

Thanks.

Con
  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000

    NorthRotorPin = 24
    EastRotorPin  = 23
    SouthRotorPin = 22
    WestRotorPin  = 21 
               
Var
    Long Channel[5]                                     'Channel Array    
    Long channelPtr
    Long MainStack[200]
    Long SecondStack[40]                                

    Long Xvector                                        'Vector Commands
    Long Yvector
    Long Zvector

    Long NorthRotor                                     'Output Vectors
    Long EastRotor
    Long SouthRotor
    Long WestRotor
         
Obj
VEX :   "VEXDemux"                        
SERVO:  "servo4"
ACCEL:  "H48C Tri-Axis Accelerometer"                                                                            
               
PUB Main

 VEX.Start (16, @Channel, @channelPtr)                  '(pin input, @channel, @syncOutPtr)

 SERVO.start(1520,NorthRotorPin,1520,EastRotorPin,1520,SouthRotorPin,1520,WestRotorPin)
 waitcnt(clkfreq / 1 + cnt)                             'required, possibly to allow other processes to initialize
 cognew(Control, @Mainstack)                    
 cognew(Calc, @Secondstack)          
                                      


PRI Control | Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,SensitivityConstant

                                                        'Channel2, up/dowm
                                                        'Channel6, hover level up/down
                                                        'Channel4, left/right
                                                        'Channel3, forward/reverse
                                                        'Channel5, Stabilization on/off
                                                        'Channel1, Rotate, if it's possible to use Channel 2
                      'Main Loop
Repeat                  
  
   if Channel[1] > 1550 or Channel[1] < 1490             'Channel2, up/dowm
    Channel2 := Channel[1] - 1550
   else
    Channel2 := 0       
   
   if Channel[2] > 1550 or Channel[2] < 1490            'Channel4, left/right
    Channel3 := Channel[2] - 1550
   else
    Channel3 := 0           
      
   if Channel[3] > 1550 or Channel[3] < 1490            'Channel3, forward/reverse
    Channel4 := Channel[3] - 1550
   else
    Channel4 := 0
                                                                                                                
   if Channel[5] > 1550                                 'Channel6, hover level up/down 
    Channel6 ++ 
   elseif Channel[5] < 1490
    Channel6 -- 
        
   if Channel[4] < 1550 or Channel[4] > 1490            'Channel5, Stabilization on/off
    Channel5 := 0                                                      
   elseif Channel[4] > 1550
    Channel5 := 1
   elseif Channel[4] < 1490
    Channel5 := -1    

  
    'Vectors represent the change in position desired, will be input for stability calculations.
    
          'X Axis Vector        
  Xvector := Channel4
            
          'Y Axis Vector
  Yvector := Channel3
            
          'Z Axis vector
  Zvector :=  Channel2 + Channel6                       'Channel6 has a value that is incremented or decremented based on the input




PRI Calc

Repeat
          'Z axis movement, (up/down)
  NorthRotor  := 1520 + Zvector         
  EastRotor   := 1520 + Zvector
  SouthRotor  := 1520 + Zvector
  WestRotor   := 1520 + Zvector

          'X axis movement, (left/right)
  EastRotor   += Xvector                                'Negation so rotors do the opposite of each other
  WestRotor   -= Xvector
                       
          'Y axis movement, (forward/reverse)
  NorthRotor  += Yvector
  SouthRotor  -= Yvector

 
   SERVO.move_to(0,NorthRotor,2)                        'Motor output commands
   SERVO.move_to(1,EastRotor,2)   
   SERVO.move_to(2,SouthRotor,2)
   SERVO.move_to(3,WestRotor,2)

JasonDorie · 2011-03-03 20:20

What does the code do differently than you're expecting it to?

I do notice that you're not clipping your "rotor" values to be within the valid servo range. If you were to center the throttle and then push full forward right, you'd likely exceed the valid input range.

You might want to use parenthesis for your multiple if statements. I can't remember if Spin gives < > higher operator precedence than OR or not. If not, you'd be getting this:

if( ( (Channel[2] > 1550) or Channel[2] ) < 1440 )

which is a lot different than this:

if( (Channel[2] > 1550) or (Channel[2] < 1440) )

That may be a red herring though.

Also, depending on how fast your Control loop updates, the value of Channel6 could quickly get a very large absolute value, which again would cause you servo problems, since it's not clipped. You might want to put a waitcnt( constant(_clkfreq / 200 + cnt ) in there or something to keep the update rate in check.

lucidshift · 2011-03-11 18:47

I should have been more clear about that, the biggest problem I've been having is that it seems to lock up after running for a while, usually about a minute or two. There doesn't seem to be a specific action that causes it to lock up either.

The rotor values aren't clipped, at least not yet. The servo object does however limit the servo values to be within an acceptable range.

For good measure I replaced the multiple OR statements with this:

if Channel[1] > 1550
Channel2 := Channel[1] - 1550
elseif Channel[1] < 1490
Channel2 := Channel[1] - 1550
else
Channel2 := 0

Figured it was best just to play it on the safe side.

The Channel6 value did increase rather quickly. I put this in the loop:

waitcnt(clkfreq / 8 + cnt)

And then I stopped getting on output for Channel6, but the rest of the loop still worked fine. Until it seems to lock up...

JasonDorie · 2011-03-11 20:24

I just noticed that your Main function launches two cogs, but never does anything else on its own. That might be an issue - You should probably make the 2nd cognew call just a direct call to calc instead. I'm not sure what the behavior of a Spin app that simply ends is, but it could be bad. I know a PASM function that just ends will keep running through memory until it loops around, and can therefore cause some really weird behavior. I'd expect Spin to behave better, but I wouldn't put money on it.

And for the record, the waitcnt will need to be a lot smaller than clkfreq/8 - that's only 8 hz, which is not likely to be fast enough. Servos update at 50hz, so start there at the very least.

lucidshift · 2011-03-17 16:03

I moved the Calc method like you suggested, and after I changed to "waitcnt(clkfreq / 200 + cnt)" everything seemed to work perfectly.

And I even got an extra Cog out of the deal.

Thanks.

JasonDorie · 2011-03-17 16:25

Glad to hear it! Now you're on to the "fun" bits.

localroger · 2011-03-19 13:45

A Spin function that returns from its first PUB should stop the cog running the Spin interpreter.

Quadcopter Control Software

Comments