Skip to content

Commit

Permalink
hard coded version 0 for Robotem rovne
Browse files Browse the repository at this point in the history
  • Loading branch information
martind committed May 5, 2014
1 parent dd260a0 commit 53cb737
Show file tree
Hide file tree
Showing 2 changed files with 292 additions and 0 deletions.
289 changes: 289 additions & 0 deletions robotem-rovne/rrv0.bas
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,289 @@
; Robotem Rovne 2014 - Version 0 (direct control without PC and Arduino board)

STOP_SERVO con -30000+258

TIMEOUT con 100

servoindex var byte
time var word ; unknown units, but it seems that 16bit TCNT counter is running
battery var word
lastCmdId var word;

servopos var word(24)
servopwr var word(24)

inputbuf var byte(24*2+2+2+1) ; cmdId, execute at, servos position,
executeAt var word
servoCmd var word(24)

chSum var byte
result var byte

loop var word


LIPOCUTOFF con 620*1024/500 ;62*1024/5.0
ADCSR = 0x30 ;start scanning AD conversion

HSERVOFEEDBACK
sethserial1 h38400

gosub stopAllServos

lastCmdId = 0

; initial wait so I can reprogram it
for loop = 0 to 20
gosub execute[STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO]
next

main
; gosub execute[0, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO, STOP_SERVO]
gosub walk
goto main

s0 var word
s1 var word
s2 var word
s3 var word
s4 var word
s5 var word
s6 var word
s7 var word
s8 var word
s9 var word
s10 var word
s11 var word
s12 var word
s13 var word
s14 var word
s15 var word
s16 var word
s17 var word
s18 var word
s19 var word
s20 var word
s21 var word
s22 var word
s23 var word

execute [s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10, s11,s12,s13,s14,s15,s16,s17,s18,s19,s20, s21,s22,s23]
gosub readServoStatus
; fill cmd
executeAt = time + 100
servoCmd(0) = s0
servoCmd(1) = s1
servoCmd(2) = s2
servoCmd(3) = s3
servoCmd(4) = s4
servoCmd(5) = s5
servoCmd(6) = s6
servoCmd(7) = s7
servoCmd(8) = s8
servoCmd(9) = s9
servoCmd(10) = s10
servoCmd(11) = s11
servoCmd(12) = s12
servoCmd(13) = s13
servoCmd(14) = s14
servoCmd(15) = s15
servoCmd(16) = s16
servoCmd(17) = s17
servoCmd(18) = s18
servoCmd(19) = s19
servoCmd(20) = s20
servoCmd(21) = s21
servoCmd(22) = s22
servoCmd(23) = s23

if battery >= LIPOCUTOFF then
gosub executeServoCmd
else
gosub stopAllServos
endif
return

;-------------------------------
stopAllServos
for servoindex = 0 to 23
hservo[servoindex\STOP_SERVO]
next
return

updateTime
prev var byte
curr var byte
prev = time & 0xFF
curr = TCB1
time = time - prev + curr
if curr < prev then
time = time + 256
endif
return


readServoStatus
gosub updateTime
battery = ADDRA>>4
for servoindex = 0 to 23
servopos( servoindex ) = hservofbpos( servoindex )
servopwr( servoindex ) = hservofbpwr( servoindex )
next
return

PACKET_START con 0xAB

sendServoStatus
dataLen var byte
dataLen = 2+2+2+24*2*2
chSum = 0
hserout s_out, [PACKET_START, dataLen]
hserout s_out, [lastCmdId &0xFF, lastCmdId>>8 ]
hserout s_out, [time &0xFF, time>>8 ]
hserout s_out, [battery & 0xFF, battery >> 8]
chSum = chSum + dataLen + (lastCmdId & 0xFF) + ((lastCmdId >> 8)&0xFF) + (time & 0xFF) + ((time >> 8)&0xFF) + (battery & 0xFF) + ((battery >> 8)&0xFF)
for servoindex = 0 to 23
hserout s_out, [servopos( servoindex )&0xFF, servopos( servoindex )>>8]
chSum = chSum + (servopos( servoindex ) & 0xFF) + (servopos( servoindex ) >> 8)
hserout s_out, [servopwr( servoindex )&0xFF, servopwr( servoindex )>>8]
chSum = chSum + (servopwr( servoindex ) & 0xFF) + (servopwr( servoindex ) >> 8)
next
hserout s_out, [-chSum]
return

receiveServoCmd
tmp var byte
i var byte
packetSize var byte
hserin s_in, timeoutException, TIMEOUT, [tmp]
while tmp <> PACKET_START
hserin s_in, timeoutException, TIMEOUT, [tmp]
wend
hserin s_in, timeoutException, TIMEOUT, [tmp]
packetSize = tmp
chSum = tmp; checksum is with length included
if packetSize = 24*2+2+2 then
for i = 0 to packetSize; including check sum
hserin s_in, timeoutException, TIMEOUT, [tmp]
inputbuf(i) = tmp
chSum = chSum + tmp
next
if chSum = 0 then
lastCmdId = inputbuf(0) + 256*inputbuf(1)
executeAt = inputbuf(2) + 256*inputbuf(3)
for i = 0 to 23
servoCmd(i) = inputbuf(4+2*i) + 256*inputbuf(5+2*i)
next
return 1
else
hserout s_out, ["ERROR chSum"]
endif
else
hserout s_out, ["ERROR packetSize"]
endif
return 0
timeoutException
hserout s_out, ["ERROR timeout"]
return 0


executeServoCmd0
hservo[servoindex\servoCmd(21)]
return

executeServoCmd
diffTime var word
gosub updateTime
diffTime = time - executeAt
while diffTime > 0x8000
gosub updateTime
diffTime = time - executeAt
wend

for servoindex = 0 to 23
hservo[servoindex\servoCmd(servoindex)]
next
return

walk
gosub execute [-8202, 6635, -29742, 123, -4682, -5016, -2288, -3063, -9839, 9539, 1144, -2816, -581, 7867, -6125, -7428, -1760, -6020, -9540, 6512, -546, -10244, 6670, 3766]
gosub execute [-8220, 7233, -29742, 123, -4682, -5016, -2288, -3063, -9188, 9468, 1179, -2764, -493, 8395, -5492, -7480, -1813, -6072, -10103, 5878, -458, -10332, 6617, 3696]
gosub execute [-8237, 7814, -29742, 123, -4682, -5016, -2288, -3063, -8536, 9398, 1232, -2711, -423, 8888, -4840, -7551, -1884, -6143, -10648, 5227, -388, -10420, 6564, 3625]
gosub execute [-8255, 8377, -29742, 123, -4682, -5016, -2288, -3063, -7868, 9328, 1284, -2640, -335, 9380, -4172, -7621, -1936, -6213, -11176, 4558, -300, -10490, 6512, 3555]
gosub execute [-8272, 8940, -29742, 123, -4682, -5016, -2288, -3063, -7199, 9240, 1337, -2588, -264, 9838, -3503, -7692, -2007, -6284, -11687, 3872, -229, -10578, 6459, 3467]
gosub execute [-8290, 9468, -29742, 123, -4682, -5016, -2288, -3063, -6495, 9169, 1390, -2535, -176, 10296, -2816, -7762, -2060, -6354, -12180, 3185, -141, -10666, 6406, 3396]
gosub execute [-8290, 9996, -29742, 123, -4682, -5016, -2288, -3063, -5756, 9099, 1443, -2482, -88, 10718, -2112, -7832, -2112, -6424, -12655, 2464, -53, -10754, 6353, 3326]
gosub execute [-8308, 10507, -29742, 123, -4682, -5016, -2288, -3063, -5016, 9028, 1478, -2412, -18, 11140, -1408, -7903, -2183, -6495, -13112, 1742, 17, -10842, 6300, 3256]
gosub execute [-8308, 11017, -29742, 123, -4682, -5016, -2288, -3063, -4242, 8958, 1531, -2359, 70, 11528, -669, -7956, -2236, -6548, -13570, 985, 105, -10912, 6248, 3185]
gosub execute [-8325, 11510, -29742, 123, -4682, -5016, -2288, -3063, -3432, 8888, 1584, -2306, 140, 11897, 52, -8026, -2288, -6618, -13992, 211, 176, -11000, 6195, 3115]
gosub execute [-8325, 11985, -29742, 123, -4682, -5016, -2288, -3063, -2588, 8800, 1619, -2236, 228, 12249, 809, -8096, -2359, -6688, -14415, -581, 264, -11088, 6142, 3027]
gosub execute [-8343, 12443, -29742, 123, -4682, -5016, -2288, -3063, -1708, 8729, 1672, -2183, 299, 12584, 1566, -8149, -2412, -6741, -14802, -1391, 334, -11159, 6089, 2956]
gosub execute [-8343, 12900, -29742, 123, -4682, -5016, -2288, -3063, -792, 8659, 1707, -2130, 387, 12900, 2323, -8220, -2464, -6812, -15172, -2236, 422, -11247, 6036, 2886]
gosub execute [-8343, 13340, -29742, 123, -4682, -5016, -2288, -3063, 176, 8571, 1760, -2060, 457, 13182, 3097, -8290, -2535, -6882, -15541, -3098, 492, -11335, 5984, 2798]
gosub execute [-8343, 13763, -29742, 123, -4682, -5016, -2288, -3063, 1179, 8500, 1795, -2007, 545, 13428, 3854, -8343, -2588, -6935, -15858, -3960, 580, -11405, 5931, 2728]
gosub execute [-8360, 14150, -29742, 123, -4682, -5016, -2288, -3063, 2217, 8430, 1848, -1954, 616, 13657, 4611, -8413, -2640, -7005, -16175, -4840, 651, -11493, 5878, 2657]
gosub execute [-8360, 14537, -29742, 123, -4682, -5016, -2288, -3063, 3291, 8342, 1883, -1884, 704, 13851, 5368, -8466, -2693, -7058, -16439, -5738, 739, -11564, 5825, 2569]
gosub execute [-8360, 14889, -29742, 123, -4682, -5016, -2288, -3063, 4400, 8272, 1918, -1831, 774, 14009, 6107, -8536, -2746, -7128, -16685, -6618, 809, -11634, 5772, 2499]
gosub execute [-8360, 15206, -29742, 123, -4682, -5016, -2288, -3063, 5526, 8184, 1971, -1760, 862, 14132, 6811, -8589, -2799, -7181, -16896, -7480, 897, -11722, 5720, 2411]
gosub execute [-8360, 15505, -29742, 123, -4682, -5016, -2288, -3063, 6652, 8113, 2006, -1708, 932, 14220, 7497, -8642, -2869, -7234, -17072, -8343, 968, -11792, 5667, 2340]
gosub execute [-8360, 15276, -29742, 123, -4682, -5016, -2288, -3063, 5667, 8113, 2006, -1708, 932, 14062, 6811, -8642, -2869, -7234, -16896, -7551, 968, -11792, 5667, 2340]
gosub execute [-8360, 15030, -29742, 123, -4682, -5016, -2288, -3063, 4681, 8113, 2006, -1708, 932, 13886, 6124, -8642, -2869, -7234, -16685, -6741, 968, -11792, 5667, 2340]
gosub execute [-8360, 14748, -29742, 123, -4682, -5016, -2288, -3063, 3696, 8113, 2006, -1708, 932, 13692, 5420, -8642, -2869, -7234, -16456, -5932, 968, -11792, 5667, 2340]
gosub execute [-8360, 14449, -29742, 123, -4682, -5016, -2288, -3063, 2728, 8113, 2006, -1708, 932, 13464, 4716, -8642, -2869, -7234, -16210, -5122, 968, -11792, 5667, 2340]
gosub execute [-8360, 14132, -29742, 123, -4682, -5016, -2288, -3063, 1760, 8113, 2006, -1708, 932, 13217, 4012, -8642, -2869, -7234, -15928, -4312, 968, -11792, 5667, 2340]
gosub execute [-8360, 13780, -29742, 123, -4682, -5016, -2288, -3063, 844, 8113, 2006, -1708, 932, 12953, 3308, -8642, -2869, -7234, -15629, -3520, 968, -11792, 5667, 2340]
gosub execute [-8360, 13428, -29742, 123, -4682, -5016, -2288, -3063, -71, 8113, 2006, -1708, 932, 12672, 2604, -8642, -2869, -7234, -15312, -2728, 968, -11792, 5667, 2340]
gosub execute [-8360, 13059, -29742, 123, -4682, -5016, -2288, -3063, -933, 8113, 2006, -1708, 932, 12355, 1918, -8642, -2869, -7234, -14978, -1972, 968, -11792, 5667, 2340]
gosub execute [-8360, 12672, -29742, 123, -4682, -5016, -2288, -3063, -1778, 8113, 2006, -1708, 932, 12038, 1232, -8642, -2869, -7234, -14626, -1215, 968, -11792, 5667, 2340]
gosub execute [-8360, 12267, -29742, 123, -4682, -5016, -2288, -3063, -2588, 8113, 2006, -1708, 932, 11704, 563, -8642, -2869, -7234, -14274, -476, 968, -11792, 5667, 2340]
gosub execute [-8360, 11862, -29742, 123, -4682, -5016, -2288, -3063, -3380, 8113, 2006, -1708, 932, 11352, -106, -8642, -2869, -7234, -13887, 246, 968, -11792, 5667, 2340]
gosub execute [-8360, 11440, -29742, 123, -4682, -5016, -2288, -3063, -4119, 8113, 2006, -1708, 932, 10982, -757, -8642, -2869, -7234, -13500, 932, 968, -11792, 5667, 2340]
gosub execute [-8360, 11000, -29742, 123, -4682, -5016, -2288, -3063, -4858, 8113, 2006, -1708, 932, 10595, -1391, -8642, -2869, -7234, -13095, 1619, 968, -11792, 5667, 2340]
gosub execute [-8360, 10560, -29742, 123, -4682, -5016, -2288, -3063, -5562, 8113, 2006, -1708, 932, 10190, -2024, -8642, -2869, -7234, -12672, 2288, 968, -11792, 5667, 2340]
gosub execute [-8360, 10102, -29742, 123, -4682, -5016, -2288, -3063, -6248, 8113, 2006, -1708, 932, 9768, -2640, -8642, -2869, -7234, -12232, 2939, 968, -11792, 5667, 2340]
gosub execute [-8360, 9627, -29742, 123, -4682, -5016, -2288, -3063, -6900, 8113, 2006, -1708, 932, 9345, -3256, -8642, -2869, -7234, -11792, 3572, 968, -11792, 5667, 2340]
gosub execute [-8360, 9169, -29742, 123, -4682, -5016, -2288, -3063, -7551, 8113, 2006, -1708, 932, 8905, -3855, -8642, -2869, -7234, -11335, 4188, 968, -11792, 5667, 2340]
gosub execute [-8360, 8676, -29742, 123, -4682, -5016, -2288, -3063, -8184, 8113, 2006, -1708, 932, 8448, -4436, -8642, -2869, -7234, -10860, 4804, 968, -11792, 5667, 2340]
gosub execute [-8360, 8184, -29742, 123, -4682, -5016, -2288, -3063, -8783, 8113, 2006, -1708, 932, 7972, -5034, -8642, -2869, -7234, -10384, 5403, 968, -11792, 5667, 2340]
gosub execute [-8360, 7673, -29742, 123, -4682, -5016, -2288, -3063, -9399, 8113, 2006, -1708, 932, 7497, -5597, -8642, -2869, -7234, -9892, 5984, 968, -11792, 5667, 2340]
gosub execute [-8941, 7620, -29742, 123, -4682, -5016, -2288, -3063, -9399, 8272, 2516, -1127, 774, 7585, -5597, -8519, -3450, -7111, -9909, 6001, 809, -12250, 5068, 2499]
gosub execute [-9504, 7568, -29742, 123, -4682, -5016, -2288, -3063, -9381, 8430, 3027, -528, 616, 7691, -5597, -8396, -4031, -6988, -9927, 6001, 651, -12690, 4435, 2657]
gosub execute [-10068, 7515, -29742, 123, -4682, -5016, -2288, -3063, -9381, 8588, 3502, 88, 457, 7779, -5597, -8272, -4647, -6864, -9944, 6001, 492, -13112, 3801, 2816]
gosub execute [-10613, 7462, -29742, 123, -4682, -5016, -2288, -3063, -9364, 8729, 3977, 704, 299, 7867, -5597, -8149, -5263, -6741, -9962, 6001, 334, -13517, 3168, 2956]
gosub execute [-11141, 7392, -29742, 123, -4682, -5016, -2288, -3063, -9346, 8888, 4435, 1337, 140, 7955, -5597, -8026, -5896, -6618, -9980, 6001, 176, -13904, 2499, 3115]
gosub execute [-11669, 7339, -29742, 123, -4682, -5016, -2288, -3063, -9346, 9028, 4892, 2006, -36, 8043, -5597, -7885, -6548, -6477, -9980, 6001, 0, -14292, 1830, 3256]
gosub execute [-12180, 7268, -29742, 123, -4682, -5016, -2288, -3063, -9328, 9187, 5315, 2675, -194, 8131, -5597, -7762, -7216, -6354, -9997, 6001, -159, -14644, 1161, 3414]
gosub execute [-12672, 7198, -29742, 123, -4682, -5016, -2288, -3063, -9328, 9328, 5737, 3361, -352, 8201, -5597, -7621, -7903, -6213, -9997, 6001, -317, -14978, 475, 3555]
gosub execute [-13165, 7128, -29742, 123, -4682, -5016, -2288, -3063, -9311, 9468, 6142, 4065, -511, 8289, -5597, -7498, -8624, -6090, -9997, 6001, -476, -15295, -229, 3696]
gosub execute [-13640, 7057, -29742, 123, -4682, -5016, -2288, -3063, -9311, 9609, 6529, 4804, -652, 8360, -5597, -7357, -9381, -5949, -9997, 6001, -616, -15594, -933, 3836]
gosub execute [-14098, 6987, -29742, 123, -4682, -5016, -2288, -3063, -9311, 9750, 6899, 5544, -810, 8430, -5615, -7216, -10156, -5808, -9997, 6001, -775, -15858, -1655, 3977]
gosub execute [-14556, 6916, -29742, 123, -4682, -5016, -2288, -3063, -9293, 9891, 7251, 6300, -968, 8500, -5615, -7076, -10965, -5668, -9997, 6001, -933, -16122, -2359, 4118]
gosub execute [-14996, 6828, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10032, 7585, 7075, -1127, 8571, -5632, -6935, -11810, -5527, -9997, 6001, -1092, -16333, -3063, 4259]
gosub execute [-15436, 6740, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10155, 7884, 7867, -1285, 8641, -5632, -6776, -12708, -5368, -9980, 6001, -1250, -16527, -3767, 4382]
gosub execute [-15858, 6652, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10278, 8184, 8676, -1444, 8694, -5650, -6636, -13623, -5228, -9980, 5984, -1408, -16703, -4453, 4505]
gosub execute [-16245, 6564, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10419, 8448, 9486, -1620, 8764, -5650, -6477, -14591, -5069, -9962, 5984, -1584, -16826, -5140, 4646]
gosub execute [-16632, 6476, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10542, 8676, 10278, -1778, 8817, -5668, -6336, -15612, -4928, -9944, 5984, -1743, -16932, -5773, 4769]
gosub execute [-17002, 6388, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10665, 8870, 11088, -1936, 8870, -5668, -6178, -16650, -4770, -9927, 5984, -1901, -17002, -6407, 4892]
gosub execute [-17354, 6283, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10788, 9046, 11862, -2077, 8923, -5685, -6020, -17724, -4612, -9909, 5984, -2042, -17037, -6988, 5016]
gosub execute [-17671, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 9187, 12619, -2236, 8976, -5703, -5861, -18815, -4453, -9892, 5984, -2200, -17037, -7533, 5121]
gosub execute [-17442, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 9011, 11827, -2236, 8976, -5703, -5861, -17829, -4453, -9892, 5984, -2200, -16879, -6847, 5121]
gosub execute [-17196, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 8800, 11017, -2236, 8976, -5703, -5861, -16844, -4453, -9892, 5984, -2200, -16703, -6160, 5121]
gosub execute [-16914, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 8571, 10208, -2236, 8976, -5703, -5861, -15858, -4453, -9892, 5984, -2200, -16509, -5456, 5121]
gosub execute [-16615, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 8324, 9398, -2236, 8976, -5703, -5861, -14890, -4453, -9892, 5984, -2200, -16280, -4752, 5121]
gosub execute [-16298, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 8043, 8588, -2236, 8976, -5703, -5861, -13922, -4453, -9892, 5984, -2200, -16034, -4048, 5121]
gosub execute [-15946, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 7744, 7796, -2236, 8976, -5703, -5861, -13007, -4453, -9892, 5984, -2200, -15770, -3344, 5121]
gosub execute [-15594, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 7427, 7004, -2236, 8976, -5703, -5861, -12092, -4453, -9892, 5984, -2200, -15488, -2640, 5121]
gosub execute [-15224, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 7092, 6248, -2236, 8976, -5703, -5861, -11229, -4453, -9892, 5984, -2200, -15172, -1954, 5121]
gosub execute [-14837, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 6740, 5491, -2236, 8976, -5703, -5861, -10384, -4453, -9892, 5984, -2200, -14855, -1268, 5121]
gosub execute [-14432, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 6388, 4752, -2236, 8976, -5703, -5861, -9575, -4453, -9892, 5984, -2200, -14520, -599, 5121]
gosub execute [-14028, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 6001, 4030, -2236, 8976, -5703, -5861, -8783, -4453, -9892, 5984, -2200, -14168, 70, 5121]
gosub execute [-13605, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 5614, 3344, -2236, 8976, -5703, -5861, -8044, -4453, -9892, 5984, -2200, -13799, 721, 5121]
gosub execute [-13165, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 5209, 2657, -2236, 8976, -5703, -5861, -7304, -4453, -9892, 5984, -2200, -13412, 1355, 5121]
gosub execute [-12725, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 4787, 1988, -2236, 8976, -5703, -5861, -6600, -4453, -9892, 5984, -2200, -13007, 1988, 5121]
gosub execute [-12268, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 4347, 1337, -2236, 8976, -5703, -5861, -5914, -4453, -9892, 5984, -2200, -12584, 2604, 5121]
gosub execute [-11792, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 3907, 704, -2236, 8976, -5703, -5861, -5263, -4453, -9892, 5984, -2200, -12162, 3220, 5121]
gosub execute [-11335, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 3449, 88, -2236, 8976, -5703, -5861, -4612, -4453, -9892, 5984, -2200, -11722, 3819, 5121]
gosub execute [-10842, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 2974, -528, -2236, 8976, -5703, -5861, -3978, -4453, -9892, 5984, -2200, -11264, 4400, 5121]
gosub execute [-10349, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 2499, -1127, -2236, 8976, -5703, -5861, -3380, -4453, -9892, 5984, -2200, -10789, 4998, 5121]
gosub execute [-9839, 6195, -29742, 123, -4682, -5016, -2288, -3063, -9293, 10894, 2006, -1708, -2236, 8976, -5703, -5861, -2764, -4453, -9892, 5984, -2200, -10314, 5561, 5121]
return
3 changes: 3 additions & 0 deletions ver1/fireant.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@ def writeCmd( self, cmd ):
if v != None and v != STOP_SERVO:
cmd2[i] = v*SERVO_DEGREE/10
buf = struct.pack( "HH"+"h"*NUM_SERVOS, self.cmdId & 0xFFFF, executeAt, *cmd2 )
# print "\tgosub execute", cmd2
self.com.write( PACKET_START )
self.com.write( chr(len(buf)) )
self.com.write( buf )
Expand Down Expand Up @@ -174,6 +175,7 @@ def setLegsXYZ( self, legXYZ, num=20 ):
self.update( cmd )

def standUp( self, interpolateFirst=True ):
print "standUp"
"prepare robot to walking height"
for z in [-0.001*i for i in xrange(120)]:
for cmd in self.setLegsXYZG( [(0.1083, 0.0625, z),(0.125, 0.0, z),(0.1083, -0.0625, z)]*2 ):
Expand All @@ -198,6 +200,7 @@ def wait( self, duration ):
self.update( cmd )

def walk( self, dist ):
print "walk"
up,down = -0.02, -0.11
s = 0.02 # step
while dist > 0:
Expand Down

0 comments on commit 53cb737

Please sign in to comment.