Reading a RC Receiver an Writing Servos
waymond91
Posts: 15
Hello All!
I am working on a little spin code to read an rc receiver and write the values to the servos. I have the read function on its own cog, and the write function on another cog. I know that both of them work- I have been able to use both loops. However, When I run both cogs together, I can see the values returned from the receiver, but the servos are not written. The only thing I can think of is that both loops are trying to access the global variables too quickly??
Ultimately the propeller will be deciding whether to let the receiver fly the plane or if it should calculate and pass its own values to the servos. But I first I need to get the servos and receiver linked up.
The idea here is that it should be easy to add new radio channels and servos if they are entered in same order. However, obviously I am not lining up the servos and the receiver channels correctly... I really am not sure what the issue is. Any ideas on this one?
Thanks for the help!
I am working on a little spin code to read an rc receiver and write the values to the servos. I have the read function on its own cog, and the write function on another cog. I know that both of them work- I have been able to use both loops. However, When I run both cogs together, I can see the values returned from the receiver, but the servos are not written. The only thing I can think of is that both loops are trying to access the global variables too quickly??
Ultimately the propeller will be deciding whether to let the receiver fly the plane or if it should calculate and pass its own values to the servos. But I first I need to get the servos and receiver linked up.
CON
_clkmode = xtal1 + pll16x 'Run at the full 80MHz
_xinfreq = 5_000_000
elevator_in = 2
ailerons_in = 6
'esc_in = 6
'rudder_in = 8
'aux_in = 10
'auto_detect = 0
elevator_out = 8
ailerons_out = 10
'esc_out = 16
'rudder_out = 18
'aux_out = 20
OBJ
pst : "Parallax Serial Terminal"
VAR
LONG stack[20], stack2[20] 'cog stacks
LONG receiver_out[8], servo_out[8] 'data arrays
byte data_flag[8], servo_count, receiver_count '0 indexed
LONG receiver_pins, servo_pins '32bit register shows which pins are reading the receiver/ writing servos
byte servo_cog
PUB main
pst.start (115_200)
cognew(receiver_read, @stack)
cognew(servo_write , @stack2)
PUB receiver_read | _pin[8], x, APIN
receiver_pins := |< elevator_in + |< ailerons_in '+ |< auto_detect '+ |< esc_in + |< rudder_in + |<aux_in
' ^^Map receiver in pins - like DIRA
x := 0
repeat 'Counts how many pins to read
_pin[x] := (>| receiver_pins)-1
receiver_pins -= |< _pin[x]
if receiver_pins == 0
receiver_count := x
quit
x++
CTRA [30..26] := 010 'PHSA only accumulates when APIN is HIGH
FRQA := 1 'Incremement PHSA by a factor of 1
repeat
repeat APIN from 0 to x
CTRA[5..0] := _pin[APIN]
WAITPEQ ( |< _pin[APIN], |< _pin[APIN], 0)
PHSA := 5
WAITPNE ( |< _pin[APIN], |< _pin[APIN], 0)
receiver_out[APIN] := (PHSA/(clkfreq/10_000_000*FRQA))
pst.dec(receiver_out[APIN])
pst.str(string(" "))
if APIN == x
pst.newline
PUB servo_write | APIN, _pin[8], x, LowTime, period, flag
servo_pins := |< elevator_out + |< ailerons_out '+ |< esc_out
dira |= servo_pins
ctra[30..26]:=100
frqa:=1
x:=0
repeat 'Counts how many pins to read
_pin[x] := (>| servo_pins)-1
servo_pins -= |< _pin[x]
if servo_pins == 0
quit
x++
LowTime:=clkfreq/200
period:=cnt
repeat
repeat APIN from 0 to x
ctra[5..0]:=_pin[APIN]
phsa:=-receiver_out[APIN] 'Send a high pulse for "position" number of clock cycles
period+=receiver_out[APIN]+LowTime 'Calculate what the system clock's value will be at the end of this cycle's period
waitcnt(period) 'Wait for the system counter to reach the "period" value (end of cycle)
The idea here is that it should be easy to add new radio channels and servos if they are entered in same order. However, obviously I am not lining up the servos and the receiver channels correctly... I really am not sure what the issue is. Any ideas on this one?
Thanks for the help!
Comments
For me it would be simpler to bit-bang servo output by selecting a minimum resolution, say it's 1µsec, and have a loop where every 1µsec you decide which pins are on and which are off. In your code I think what I'm seeing is that it delays for some period for each pin somehow. Does that work when ch1 is on longer than ch2, or vice versa? Come to think of it, why not just use the servo32x7 object for servo output? (on the other hand it can be a good learning experience to try to write something like it yourself but you can see how it was done in that library)
Also there's probably a slightly simpler and more modular way to pass in / process the list of pins to monitor/set. Rather than a loop to determine the number of channels, you could have an array that contains the pin numbers, and -1 indicates end of the array. Something like
pin[0] := 2
pin[1] := 6
pin[2] := 8
pin[3] := 10
pin[4] := -1
Then loop thru to do whatever until you hit -1 and use (|<pin[x]) to get the actual pin of each. Alternatively, pass in the array and a channel count variable. I have seen (and used) either approach (though not in this specific context) many times.
It's nice to avoid complex duplicated code if possible to avoid bugs as the program grows and gets more complex.
Agreed. If you're not doing this just as a learning experience (a good enough reason IMO), you could use the Servo32v7 object for the servo output and this object for the receiver input.
It's been over a week since I've mentioned a few of my servo demos, so here they are.
A QuickStart board controlling 32 servos.
My QuickStart servo tester.
Both of the above programs use the Servo32v7 object Michael mentioned.
Your "main" method doesn't terminate "correctly".
Cog zero (assuming this is the top object) is allowed to stop on its own without being instructed to do so. I'm not sure what problems this can present but it is considered "bad form" (or something like that). I think letting a cog die this way can create stack problems? At least that's my guess as to why it's "bad form". Hopefully someone will enlighten us why it's not good to do it this way.
Here's the main method changed to make use of cog zero.
I suppose this could be causing your problem after all. Does the cog launching another cog need to stay alive long enough for the other cog to finish launching? I doubt it. Just guessing. I do know that it does take time to launch a cog and you can't assume the code in the launched cog is immediately active. I once timed the delay from cog launch to when the cog was usable but I don't remember how long this delay is (I'm pretty sure it's less than 1/100 second).
I am still new to the pro board. I was a little anxious before, but looks like I can exercise a lot more control than I could with the ol' arduino lol.
This code was a good to learn how the counters work.
I ended up doing something like this:
CON _clkmode = xtal1 + pll16x 'Run at the full 80MHz _xinfreq = 5_000_000 elevator_in = 2 ailerons_in = 3 'esc_in = 6 'rudder_in = 8 'aux_in = 10 'auto_detect = 0 elevator_out = 8 ailerons_out = 9 'esc_out = 16 'rudder_out = 18 'aux_out = 20 OBJ pst : "Parallax Serial Terminal" VAR LONG stack[20], stack2[20] 'cog stacks long data[2] PUB main pst.start (115_200) cognew(receiver_read, @stack) cognew(servo , @stack2) PUB receiver_read | x CTRA [30..26] := %11010 'PHSA only accumulates when APIN is HIGH FRQA := 1 'Incremement PHSA by a factor of 1 repeat repeat x from 2 to 3 CTRA[5..0] := x WAITPEQ ( |< x, |< x, 0) PHSA := 5 WAITPNE ( |< x, |< x, 0) data[x-1] := (PHSA/(clkfreq/10_000_000*FRQA))*10 PUB servo | LowTime, period, x frqa := 1 ctra[30..26]:=%00100 LowTime:=clkfreq/100 period:=cnt repeat repeat x from 8 to 9 dira[x] := 1 ctra[5..0]:= x phsa:=-data[x-7] period+=data[x-7]+LowTime waitcnt(period)It would be possible to use only even/odd pins (this has appeal to me because I will be soldering a little board on top of the quickstart)
you could increment x by a factor of 2.
Although I guess I am still not terminating the initial cog correctly? Is it ok to use the startup cog to call two new cogs and then let the first one die?
That's good to know. I suppose my remembering it being "bad form" was just someone's stylistic preference.
Personally it bugs me to see a cog end that way. I think my objection is it obscures the number of cogs being used a bit.
Thanks all for the help!!!