Robot program issues! Please HELP!
albert
Posts: 10
Hello, im having some issues about this robot program. We want the pulse to work SIMULTANEOUSLY, one that becomes high and the other one low. We are working on Pneumatic air muscles and we have 3 DOF and on each DOF there are two muscles, one that drags the arm upp while the other muslce release air. hope i explained this fairly good.
Albert
[code]
CON
TOLERANS = 5 'tolerance as a percentage of what is considered as the center position of the lever
MINPULSE = 120 'shortest pulse duration (in clock pulses)
PERIOD = 12000 'period time (in clock pulses)
VAR
long stack[50]
byte cog
long pulselength1, pulselength2, intervallstart, intervallstop
long temp
PUB Start(pin1, pin2, readaddr) :success
Stop
success := cog := cognew(Move(pin1, pin2, readaddr), @stack) +1
PUB Stop
if cog
cogstop(cog~ -1)
PRI Move(pin1, pin2, readaddr)
waitcnt(clkfreq + cnt) 'method to send pulses to control one degree of freedom
intervallstart := intervallstop := LONG[readaddr] 'initiates field for what should be regarded as middle
intervallstart := intervallstart - (TOLERANS * intervallstart/100)
intervallstop := intervallstop + (TOLERANS * intervallstop/100)
pulselength1 := PERIOD - MINPULSE 'initiates the pulse duration of the PWM(Pulse Width Modulation)
pulselength2 := MINPULSE
dira[23..16]~~ ''sets pin to output
dira[pin1] := dira[pin2] := 1
ctra[30..26] := ctrb[30..26] := %11111 'starts two timers that count up continuously 1 every clock pulse
frqa := frqb := 1
phsa := phsb := 0
repeat
waitcnt(clkfreq + cnt)
temp := LONG[readaddr] 'copies the value of readaddr to temp
if temp < intervallstart 'check if the lever is in the area during the middle
if pulselength1 > MINPULSE 'verify that the pulse duration did not come to the end
pulselength1-- 'change the pulse length
pulselength2++
else
if temp > intervallstop 'check if the lever is in the area over the middle
if pulselength2 > MINPULSE 'verify that the pulse duration did not come to the end
pulselength1++ 'changes the pulse lengths
pulselength2--
{ outa[23..16]~ 'changes the pulse lengths
outa[16+(pulselength1/1600)]~~
outa[16+(pulselength2/1600)]~~
}
if ina[pin1] AND (phsa => pulselength1) 'check if PIN1 been high long enough
outa[pin1]~ 'sets PIN1 high and resets the counter a
phsa~
else
if (NOT ina[pin1]) AND (phsa => (PERIOD - pulselength1))
outa[pin1]~~
phsa~
if ina[pin2] AND (phsb => pulselength2) 'check if PIN2 has been high long enough
outa[pin2]~ 'reset the PIN2 and counters b
phsb~
else
if (NOT ina[pin2]) AND (phsb => (PERIOD - pulselength2)) 'check if PIN2 has been low long enough
outa[pin2]~~ 'ettst
Albert
[code]
CON
TOLERANS = 5 'tolerance as a percentage of what is considered as the center position of the lever
MINPULSE = 120 'shortest pulse duration (in clock pulses)
PERIOD = 12000 'period time (in clock pulses)
VAR
long stack[50]
byte cog
long pulselength1, pulselength2, intervallstart, intervallstop
long temp
PUB Start(pin1, pin2, readaddr) :success
Stop
success := cog := cognew(Move(pin1, pin2, readaddr), @stack) +1
PUB Stop
if cog
cogstop(cog~ -1)
PRI Move(pin1, pin2, readaddr)
waitcnt(clkfreq + cnt) 'method to send pulses to control one degree of freedom
intervallstart := intervallstop := LONG[readaddr] 'initiates field for what should be regarded as middle
intervallstart := intervallstart - (TOLERANS * intervallstart/100)
intervallstop := intervallstop + (TOLERANS * intervallstop/100)
pulselength1 := PERIOD - MINPULSE 'initiates the pulse duration of the PWM(Pulse Width Modulation)
pulselength2 := MINPULSE
dira[23..16]~~ ''sets pin to output
dira[pin1] := dira[pin2] := 1
ctra[30..26] := ctrb[30..26] := %11111 'starts two timers that count up continuously 1 every clock pulse
frqa := frqb := 1
phsa := phsb := 0
repeat
waitcnt(clkfreq + cnt)
temp := LONG[readaddr] 'copies the value of readaddr to temp
if temp < intervallstart 'check if the lever is in the area during the middle
if pulselength1 > MINPULSE 'verify that the pulse duration did not come to the end
pulselength1-- 'change the pulse length
pulselength2++
else
if temp > intervallstop 'check if the lever is in the area over the middle
if pulselength2 > MINPULSE 'verify that the pulse duration did not come to the end
pulselength1++ 'changes the pulse lengths
pulselength2--
{ outa[23..16]~ 'changes the pulse lengths
outa[16+(pulselength1/1600)]~~
outa[16+(pulselength2/1600)]~~
}
if ina[pin1] AND (phsa => pulselength1) 'check if PIN1 been high long enough
outa[pin1]~ 'sets PIN1 high and resets the counter a
phsa~
else
if (NOT ina[pin1]) AND (phsa => (PERIOD - pulselength1))
outa[pin1]~~
phsa~
if ina[pin2] AND (phsb => pulselength2) 'check if PIN2 has been high long enough
outa[pin2]~ 'reset the PIN2 and counters b
phsb~
else
if (NOT ina[pin2]) AND (phsb => (PERIOD - pulselength2)) 'check if PIN2 has been low long enough
outa[pin2]~~ 'ettst
Comments