This is the main routine that sends the data to the 3 receivers on 3 separate lines.
Code:
Clear
@ DEVICE pic16F684, WDT_OFF
adcon0 = 7
adcon1 = 7
ansel = 0
define OSC 4
trisc = %000111
define CHAR_PACING 1000
t1 var byte[6] 'angle of servo at body joint
t2 var byte[6] 'angle of servo at leg
t3 var byte[6] 'angle of servo at thigh
i var byte
start var byte
ender var byte
a var byte
k var byte
j var byte
l var word
rec var byte
control var byte
sta var byte
fin var byte
ste var byte
control = 0
speed var byte
rec = 0
Init:
For i = 0 to 5
t2[i] = 110
t3[i] = 110
t1[i] = 90
next i
Gosub Send
Main: 'Main makes the robot sit then stand then step forward 5 times
' and then turn 5 times and then backward 5 times then sit.
For l = 0 to 1
gosub sit
next l
for l = 0 to 1000
pauseus 1
next l
For l = 0 to 1
gosub stand
next l
for l = 0 to 1000
pauseus 1
next l
For l = 0 to 5
speed = 2
gosub forward2
next l
for l = 0 to 5
speed = 2
gosub turn
next l
for l = 0 to 5
speed = - 2
gosub Backward
next l
for l = 0 to 1
gosub sit
next l
while 0 = 0 'robot remains sitting indefinately
wend
goto main
Forward2: 'moves every leg individualy forward while the rest move backward slowly.
For i = 0 to 5 step 1
t2[i] = 130
t3[i] = 130
porta.3 = 0
for j = 0 to 5 step 1
For k = 0 to 50
pauseus 1
next k
select case i
case 0
t1[0] = t1[0] + 5 * speed
t1[1] = t1[1] - speed
t1[2] = t1[2] - speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
Case 1
t1[0] = t1[0] - speed
t1[1] = t1[1] + 5 * speed
t1[2] = t1[2] - speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
case 2
t1[0] = t1[0] - speed
t1[1] = t1[1] - speed
t1[2] = t1[2] + 5 * speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
case 3
t1[0] = t1[0] - speed
t1[1] = t1[1] - speed
t1[2] = t1[2] - speed
t1[3] = t1[3] + 5 * speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
case 4
t1[0] = t1[0] - speed
t1[1] = t1[1] - speed
t1[2] = t1[2] - speed
t1[3] = t1[3] - speed
t1[4] = t1[4] + 5 * speed
t1[5] = t1[5] - speed
case 5
t1[0] = t1[0] - speed
t1[1] = t1[1] - speed
t1[2] = t1[2] - speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] + 5 * speed
end select
Gosub Send 'goes to the serout2 routines sending the position of the legs (t1 t2 and t3)
next j
t2[i] = 110
t3[i] = 110
Gosub Send
next i
return
Backward: 'basically the same as forward but in reverse.
For i = 5 to 0 step - 1
t2[i] = 130
t3[i] = 130
porta.3 = 0
for j = 0 to 5 step 1
For k = 0 to 50
pauseus 1
next k
select case i
case 0
t1[0] = t1[0] + 5 * speed
t1[1] = t1[1] - speed
t1[2] = t1[2] - speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
Case 1
t1[0] = t1[0] - speed
t1[1] = t1[1] + 5 * speed
t1[2] = t1[2] - speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
case 2
t1[0] = t1[0] - speed
t1[1] = t1[1] - speed
t1[2] = t1[2] + 5 * speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
case 3
t1[0] = t1[0] - speed
t1[1] = t1[1] - speed
t1[2] = t1[2] - speed
t1[3] = t1[3] + 5 * speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
case 4
t1[0] = t1[0] - speed
t1[1] = t1[1] - speed
t1[2] = t1[2] - speed
t1[3] = t1[3] - speed
t1[4] = t1[4] + 5 * speed
t1[5] = t1[5] - speed
case 5
t1[0] = t1[0] - speed
t1[1] = t1[1] - speed
t1[2] = t1[2] - speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] + 5 * speed
end select
Gosub Send
next j
t2[i] = 110
t3[i] = 110
Gosub Send
next i
return
Turn: 'similar to others but changes the direction of a side.
For i = 5 to 0 step - 1
t2[i] = 130
t3[i] = 130
porta.3 = 0
for j = 0 to 5 step 1
For k = 0 to 50
pauseus 1
next k
select case i
case 0
t1[0] = t1[0] - 5 * speed
t1[1] = t1[1] + speed
t1[2] = t1[2] + speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
Case 1
t1[0] = t1[0] + speed
t1[1] = t1[1] - 5 * speed
t1[2] = t1[2] + speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
case 2
t1[0] = t1[0] + speed
t1[1] = t1[1] + speed
t1[2] = t1[2] - 5 * speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
case 3
t1[0] = t1[0] + speed
t1[1] = t1[1] + speed
t1[2] = t1[2] + speed
t1[3] = t1[3] + 5 * speed
t1[4] = t1[4] - speed
t1[5] = t1[5] - speed
case 4
t1[0] = t1[0] + speed
t1[1] = t1[1] + speed
t1[2] = t1[2] + speed
t1[3] = t1[3] - speed
t1[4] = t1[4] + 5 * speed
t1[5] = t1[5] - speed
case 5
t1[0] = t1[0] + speed
t1[1] = t1[1] + speed
t1[2] = t1[2] + speed
t1[3] = t1[3] - speed
t1[4] = t1[4] - speed
t1[5] = t1[5] + 5 * speed
end select
Gosub Send
next j
t2[i] = 110
t3[i] = 110
Gosub Send
next i
return
sit: 'makes the legs go up to set the robot down.
while t3[0] < 175
t3[0] = t3[0] + 2
t2[0] = t2[0] + 1
t3[1] = t3[1] + 2
t2[1] = t2[1] + 1
t3[2] = t3[2] + 2
t2[2] = t2[2] + 1
t3[3] = t3[3] + 2
t2[3] = t2[3] + 1
t3[4] = t3[4] + 2
t2[4] = t2[4] + 1
t3[5] = t3[5] + 2
t2[5] = t2[5] + 1
Gosub Send
wend
return
Stand: 'makes legs go down to stand robot up
while t3[0] > 110
t3[0] = t3[0] - 2
t2[0] = t2[0] - 1
t3[1] = t3[1] - 2
t2[1] = t2[1] - 1
t3[2] = t3[2] - 2
t2[2] = t2[2] - 1
t3[3] = t3[3] - 2
t2[3] = t2[3] - 1
t3[4] = t3[4] - 2
t2[4] = t2[4] - 1
t3[5] = t3[5] - 2
t2[5] = t2[5] - 1
Gosub Send
wend
return
Send: 'the send routine sends t1,t2,and t3 to the 3 servo controllers.
While portc.0 = 1
wend
serout2 portc.3,32852,["@", STR t1\6]
While portc.2 = 1
wend
serout2 portc.4,32852,["@", STR t2\6]
while portc.1 = 1
wend
serout2 portc.5,32852,["@", STR t3\6]
Return
end
This is the receiver I am using. The three receivers all use the same code.
Code:
'****************************************************************
'* Name : Hexapod.BAS *
'* Author : Corbet Peters *
'* Notice : Copyright (c) 2011 *
'* : All Rights Reserved *
'* Date : 4/26/2011 *
'* Version : 1.0 *
'* Notes : *
'* : *
'****************************************************************
Clear
@ DEVICE pic16F84, WDT_OFF
define osc 4
trisa = %000000
trisb = $00
low portb
t1pulse var byte[6]
servos var portb
servos[0] = portb.1
servos[1] = portb.2
servos[2] = portb.3
servos[3] = portb.4
servos[4] = portb.5
servos[5] = portb.6
t1pulse[0] = 90
t1pulse[1] = 90
t1pulse[2] = 90
t1pulse[3] = 90
t1pulse[4] = 90
t1pulse[5] = 90
porta.1 = 1
porta.2 = 0
trisa.3 = 1
i var byte
Main:
porta.2 = 0 'turns on a transistor that brings pin on sender high
'to stop it from sending the serial
porta.2 = 1 'turns transistor on to bring sender pin low to
'tell sender to send
pulsout portb.3, t1pulse[0]+60
pulsout portb.2, t1pulse[1]+60
pulsout portb.1, t1pulse[2]+60
pulsout portb.4, 240 - t1pulse[3]
pulsout portb.5, 240 - t1pulse[4]
pulsout portb.6, 240 - t1pulse[5]
pulsout portb.7, 240 - t1pulse[5]
Serin2 porta.3,32852,10,comfail,[wait ("@"), str t1pulse\6] 'if timeout length
' is longer than the time between the sends
'on the sender, this line hangs up and chip
'acts weird like turning porta.2 on but not
' sending the pulses or turn on servo pins
' and never turn off
porta.2 = 0 'tell sender to stop sending
goto main 'loop to main
Comfail:
porta.2 = 0
goto main
Sorry for the delay, I have been busy and I wanted a chance to comment a little. Any questions please ask. Any suggestions please post!
Bookmarks