Quadcopter Control Software
lucidshift
Posts: 3
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.
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)
Comments
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.
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...
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.
And I even got an extra Cog out of the deal.
Thanks.