From 8bf6f432e200f4d3fa494081837915b15015ab65 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Thu, 30 Jan 2020 21:46:10 +0100 Subject: [PATCH] avr nano: Added Manipulator 12V enable/disable --- avr/nano/Makefile | 3 +- avr/nano/main.c | 21 +- avr/nano/main.hex | 658 +++++++++++++++++++++++---------------------- scripts/wt_node.py | 8 +- 4 files changed, 354 insertions(+), 336 deletions(-) diff --git a/avr/nano/Makefile b/avr/nano/Makefile index 1587f45..d63e41f 100644 --- a/avr/nano/Makefile +++ b/avr/nano/Makefile @@ -321,7 +321,8 @@ gccversion : # Program the device. program: $(TARGET).hex $(TARGET).eep - $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) + #$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) + ../../scripts/pyshared/bootloader.py -b -j 0x52 $(TARGET).hex diff --git a/avr/nano/main.c b/avr/nano/main.c index 935ce49..07e1871 100644 --- a/avr/nano/main.c +++ b/avr/nano/main.c @@ -28,6 +28,7 @@ * 0x0C Distance forward2 LSB * 0x0D Current MSB * 0x0E Current LSB + * 0x0F Manipulator 12V * * 0x15 Distance forward1 MSB (read only) * 0x16 Distance forward1 LSB (read only) @@ -52,10 +53,10 @@ * A7 ADC7 * D0 PD0/RX * D1 PD1/TX - * D2 PD2/IND0 - * D3 PD3/IND1 - * D4 PD4/T0 - * D5 PD5/T1 + * D2 PD2/IND0 dist_fwd1 + * D3 PD3/IND1 dist_bwd + * D4 PD4/T0 dist_fwd2 + * D5 PD5/T1 Manipulator 12V * D6 PD6/AIN0 * D7 PD7/AIN1 * D8 PB0/ICP @@ -101,12 +102,17 @@ ISR(TWI_vect) ireg = TWDR; if (ireg == 0x05) start_dist_fwd1=1; - if (ireg == 0x07) start_dist_bwd=1; - if (ireg == 0x0b) start_dist_fwd2=1; + else if (ireg == 0x07) start_dist_bwd=1; + else if (ireg == 0x0b) start_dist_fwd2=1; ireg--; // because we do ireg++ below TWI_ACK; break; + case 0x0f: // Manipulator 12V + if (TWDR) PORTD |= (1 << 5); + else PORTD &= ~(1 << 5); + TWI_ACK; + break; case 0xff: // bootloader bootloader = TWDR; default: @@ -346,6 +352,9 @@ int main(void) { setup_uart(9600); uart_setup_stdout(); + DDRD = (1 << 5); + PORTD = 0x0; + // I2C TWAR = 0x52; TWI_ACK; diff --git a/avr/nano/main.hex b/avr/nano/main.hex index a1a442b..9ddffaf 100644 --- a/avr/nano/main.hex +++ b/avr/nano/main.hex @@ -1,337 +1,339 @@ -:100000000C945F000C9466010C94B0010C947C007D -:100010000C947C000C94FA010C947C000C947C00F1 +:100000000C945F000C9471010C94BB010C947C0067 +:100010000C947C000C9405020C947C000C947C00E5 :100020000C947C000C947C000C947C000C947C0060 :100030000C947C000C947C000C947C000C947C0050 -:100040000C947C000C947C000C945B020C947C005F +:100040000C947C000C947C000C9466020C947C0054 :100050000C947C000C947C000C947C000C947C0030 -:100060000C947E000C947C00E6000F01EB000F0165 -:1000700000010F0105010F01F6000F010A010F0138 -:10008000FB000F01150115011501150115011501E1 -:1000900000010F0105010F010A010F010D0A537440 +:100060000C947E000C947C00F1001A01F6001A0139 +:100070000B011A0110011A0101011A0115011A01DF +:1000800006011A0120012001200120012001200188 +:100090000B011A0110011A0115011A010D0A5374FE :1000A0006172740D0A0045464765666700202B2D76 :1000B0002E30313233343536373839680000112468 :1000C0001FBECFEFD8E0DEBFCDBF21E0A6E0B1E09C :1000D00001C01D92A739B207E1F711E0A0E0B1E03D -:1000E000E6EEF4E102C005900D92A630B107D9F713 -:1000F0000E9462030C94710A0C9400001F920F92EC +:1000E000E2E0F5E102C005900D92A630B107D9F724 +:1000F0000E946D030C947F0A0C9400001F920F92D3 :100100000FB60F9211248F939F93AF93BF93EF93EA -:10011000FF938091B900887F803A09F491C028F458 -:10012000803669F0803889F08DC0883B09F441C081 -:10013000883F09F48AC0883A09F084C03AC085EC47 -:100140008093BC001092210180C0809121018823FE -:1001500039F08F3F29F58091BB008093200120C0AA -:100160008091BB008093210180912101853019F499 -:1001700081E08093150180912101873019F481E09D -:1001800080931301809121018B3019F481E08093D9 -:1001900014018091210181508093210185EC01C0DF -:1001A00085E88093BC00809121018F3F09F042C017 -:1001B0004CC0E09121018E2F90E0FC013197EA3193 -:1001C000F10598F5EC5CFF4F0C94990580911E01A8 -:1001D00090911F0104C080911C0190911D0180939A -:1001E0000C0190930D019093BB001DC080911101F3 -:1001F00090911201F4CF80910F0190911001EFCFF7 -:1002000080911A0190911B01EACF80911601909183 -:100210001701E5CF8091180190911901E0CF8091ED -:100220000C018093BB0085EC03C01092BB0085E8F5 -:100230008093BC00809121018F5F8093210105C0D4 -:1002400085EC01C085ED8093BC00FF91EF91BF91DB -:10025000AF919F918F910F900FBE0F901F901895A7 -:1002600097E890937A0080937C0080917C00806472 -:1002700080937C0080917A00806480937A008091E2 -:100280007A0086FDFCCF809178009091790025E07E -:1002900080E090E030917A00306430937A003091C1 -:1002A0007A0036FDFCCF4091780050917900840FA0 -:1002B000951F215079F720917A002F7720937A00AB -:1002C00065E070E00E946605CB0108951F920F92D1 -:1002D0000FB60F9211242F933F934F935F936F9319 -:1002E0007F938F939F93AF93BF93EF93FF9360910F -:1002F0008400709185004A9B05C070930B01609348 -:100300000A011DC080910A0190910B01681B790BB5 -:1003100080E090E00E94060727EE3CE840E350E4CE -:100320000E94940720E030E040E05FE30E94F9057E -:100330000E94D70670931B0160931A01E898FF9101 -:10034000EF91BF91AF919F918F917F916F915F914D -:100350004F913F912F910F900FBE0F901F901895C6 -:100360001F920F920FB60F9211242F933F934F932A -:100370005F936F937F938F939F93AF93BF93EF930D -:10038000FF9360918400709185004B9B05C0709332 -:100390000901609308011DC0809108019091090135 -:1003A000681B790B80E090E00E94060727EE3CE88E -:1003B00040E350E40E94940720E030E040E05FE337 -:1003C0000E94F9050E94D7067093170160931601E9 -:1003D000E998FF91EF91BF91AF919F918F917F919C -:1003E0006F915F914F913F912F910F900FBE0F90A2 -:1003F0001F9018951F920F920FB60F9211242F93F2 -:100400003F934F935F936F937F938F939F93AF939C -:10041000BF93EF93FF93CF93DF931F92CDB7DEB7D8 -:1004200089B1898390910E01892784FF2DC0609145 -:10043000840070918500898184FF05C07093070155 -:100440006093060121C08091060190910701681B0D -:10045000790B80E090E00E94060727EE3CE840E33D -:1004600050E40E94940720E030E040E05FE30E9407 -:10047000F9050E94D70670931901609318018091C5 -:100480006D008F7E80936D00898180930E010F90A7 -:10049000DF91CF91FF91EF91BF91AF919F918F919C -:1004A0007F916F915F914F913F912F910F900FBE70 -:1004B0000F901F9018951F920F920FB60F92112454 -:1004C0008F939F93EF93FF938091C00087FF17C096 -:1004D000E0912401F09125018091C600808381E0A4 -:1004E000E938F80730F43196F0932501E0932401C0 -:1004F00006C086E291E09093250180932401FF914C -:10050000EF919F918F910F900FBE0F901F901895B4 -:100510009C01E1ECF0E088E1808380818068808349 -:1005200086E08093C200C901A0E0B0E09C01AD016B -:1005300084E0220F331F441F551F8A95D1F760E0D6 -:1005400074E284EF90E00E947A05DA01C901019714 -:10055000A109B109892F9A2FAB2FBB27A7FDBA9507 -:100560008093C50021502093C40086E291E09093CF -:100570002301809322019093250180932401089503 -:100580002091C00025FFFCCFFC0180818093C60034 -:100590000895CF93DF931F92CDB7DEB78983CE0145 -:1005A00001960E94C00280E090E00F90DF91CF9111 -:1005B0000895CF93DF93EC01CE012196FC012081B9 -:1005C000222319F00E94C002F7CFDF91CF91089546 -:1005D0008091C00087FFFCCF8091C60090E0089515 -:1005E0000C94E80260EF72E089EC92E00C940C0845 -:1005F000CF93DF93FC01E253FF4FA081B181EC0167 -:10060000C453DF4F28813981A217B307B9F0209175 -:10061000C00025FF13C02C912093C600208131819A -:10062000AC0145535F4F2417350718F42F5F3F4F38 -:1006300003C09C0128593F4F31832083E091220160 -:10064000F091230120912401309125012E173F07BD -:10065000B1F1EC01CC59DF4FA881B98120812C93F5 -:10066000688179819C012B523F4FD9013C91DB017C -:100670002C91321307C0AC014C525F4FDA012C9120 -:100680002F5F2C939C012D593F4F6217730728F45D -:10069000DB011196B983A88302C099838883B1E0F6 -:1006A000E938FB0730F43196F0932301E0932201FF -:1006B00006C086E291E09093230180932201DF91AE -:1006C000CF9108951092200180E895E20E9488025F -:1006D0000E94F20282E58093BA0085EC8093BC0010 -:1006E0001092800084E08093810095E090936900EF -:1006F000809368008CE990E09F938F930E948F080D -:1007000083B7817F83BF78940F900F90C8E1D8E0C2 -:10071000809121018930E1F068F4813089F08330E3 -:1007200009F060C081E00E94300190931D01809328 -:100730001C0158C08D3009F18F3F09F43FC052C0F1 -:1007400080E00E94300190931F0180931E014AC0F7 -:1007500082E00E943001BC0180E090E00E94060728 -:1007600028E431EE4AEB5FE30E9494070E94D7062B -:10077000709312016093110135C083E00E94300133 -:10078000BC0180E090E00E94060725E13CE840EAD9 -:1007900050E40E9494072CEE31E741E054E40E94BB -:1007A000F80522EF38E941E05EE30E9465060E9409 -:1007B000D7067093100160930F0114C0809120013F -:1007C000853A81F4F8946BE780E090E00E94630A38 -:1007D000F999FECF0FB6F894A895C09360000FBEAC -:1007E000D093600080911501882391F0109215013B -:1007F00010921B0110921A01529A5A9A85E38A9517 -:10080000F1F700005A9852988CB381708CBBE89A2B -:1008100080911301882391F0109213011092170117 -:1008200010921601539A5B9A85E38A95F1F70000BE -:100830005B9853988CB382708CBBE99A80911401B9 -:100840008823B1F01092140110921901109218012E -:10085000549A5C9A85E38A95F1F700005C98549865 -:1008600089B180930E0180916D00806180936D004D -:1008700083B7816083BF889583B78E7F83BF48CF5E -:10088000ACE0B0E0E6E4F4E00C94A3056C015B019D -:100890007A01FC0117821682838181FF0EC1CE018D -:1008A00001964C01F6010381F50103FD159103FF4B -:1008B00011915F01112309F4FCC0153249F403FDC5 -:1008C000159103FF11915F01153211F020E01BC05B -:1008D000B601812F90E00E94570837017301E2CFE3 -:1008E000112309F4E6C0612F70E08DEA90E02C87B7 -:1008F0000E9401082C85892B49F0F50103FD159113 -:1009000003FF11915F0127FFEBCF09C0133229F0DC -:100910001C3609F0D5C02068F0CF20E1EECF022FC1 -:10092000112309F4C6C0612F70E086EA90E02C879D -:100930000E9401082C85892B41F03701F4E06F0EED -:10094000711CB6018FE390E011C0133639F01337F4 -:1009500081F0133519F5022F01600BC0370182E0D9 -:10096000680E711CB601F701808191810E945708C1 -:10097000B5CF3701F2E06F0E711CF701E080F18016 -:10098000F70100FD859100FF81917F01882309F423 -:10099000A5CFB60190E00E945708F2CF143611F0AF -:1009A000193629F5370127FF09C0F4E06F0E711CD5 -:1009B000F70160817181828193810AC0F2E06F0E3C -:1009C000711CF70160817181072E000C880B990B57 -:1009D000022F0F7E97FF08C09095809570956195C6 -:1009E0007F4F8F4F9F4F00642AE030E033C01037B5 -:1009F00099F040F41835B1F01F3609F05AC028E0DC -:100A000030E014C0153719F0183741F052C0022FEA -:100A10000F7E2AE030E00AC0022F0061046220E16C -:100A200030E004C0022F046020E132E0370107FF0C -:100A300009C0F4E06F0E711CF70160817181828141 -:100A4000938108C0F2E06F0E711CF7016081718123 -:100A500080E090E0A4010E94B508182F181906FF45 -:100A600005C0B6018DE290E00E94570804FF15C052 -:100A7000FE01E10FF11D8081803379F0B60180E342 -:100A800090E00E94570802FF08C00072802F90E09B -:100A9000B601885A9F4F0E9457081150F401E10F88 -:100AA000F11D8081B60190E00E9457081111F5CF29 -:100AB00015CFF6018681978105C08FEF9FEF02C0A9 -:100AC000022F31CF2C96EEE00C94BF05AA1BBB1B66 -:100AD00051E107C0AA1FBB1FA617B70710F0A61B3E -:100AE000B70B881F991F5A95A9F780959095BC015F -:100AF000CD010895052E97FB1EF400940E949105E8 -:100B000057FD07D00E94D60507FC03D04EF40C9485 -:100B1000910550954095309521953F4F4F4F5F4F30 -:100B2000089590958095709561957F4F8F4F9F4F59 -:100B30000895EE0FFF1F0590F491E02D09942F9278 -:100B40003F924F925F926F927F928F929F92AF925D -:100B5000BF92CF92DF92EF92FF920F931F93CF93AA -:100B6000DF93CDB7DEB7CA1BDB0B0FB6F894DEBF41 -:100B70000FBECDBF09942A88398848885F846E8467 -:100B80007D848C849B84AA84B984C884DF80EE80B1 -:100B9000FD800C811B81AA81B981CE0FD11D0FB6BA -:100BA000F894DEBF0FBECDBFED010895A1E21A2E6D -:100BB000AA1BBB1BFD010DC0AA1FBB1FEE1FFF1F01 -:100BC000A217B307E407F50720F0A21BB30BE40B51 -:100BD000F50B661F771F881F991F1A9469F7609598 -:100BE0007095809590959B01AC01BD01CF01089552 -:100BF0005058BB27AA270E9410060C945A070E943F -:100C00004C0738F00E94530720F039F49F3F19F445 -:100C100026F40C9449070EF4E095E7FB0C94430787 -:100C2000E92F0E946B0758F3BA176207730784070E -:100C3000950720F079F4A6F50C948D070EF4E09555 -:100C40000B2EBA2FA02D0B01B90190010C01CA0186 -:100C5000A0011124FF27591B99F0593F50F4503E31 -:100C600068F11A16F040A22F232F342F4427585F23 -:100C7000F3CF469537952795A795F0405395C9F73B -:100C80007EF41F16BA0B620B730B840BBAF09150F3 -:100C9000A1F0FF0FBB1F661F771F881FC2F70EC092 -:100CA000BA0F621F731F841F48F487957795679565 -:100CB000B795F7959E3F08F0B0CF9395880F08F051 -:100CC0009927EE0F9795879508950E9479060C94C1 -:100CD0005A070E94530758F00E944C0740F029F42D -:100CE0005F3F29F00C94430751110C948E070C942C -:100CF00049070E946B0768F39923B1F3552391F3D9 -:100D0000951B550BBB27AA2762177307840738F07A -:100D10009F5F5F4F220F331F441FAA1FA9F335D0D7 -:100D20000E2E3AF0E0E832D091505040E695001C8B -:100D3000CAF72BD0FE2F29D0660F771F881FBB1F45 -:100D4000261737074807AB07B0E809F0BB0B802D23 -:100D5000BF01FF2793585F4F3AF09E3F510578F04F -:100D60000C9443070C948E075F3FE4F3983ED4F352 -:100D7000869577956795B795F7959F5FC9F7880F23 -:100D8000911D9695879597F90895E1E0660F771F75 -:100D9000881FBB1F621773078407BA0720F0621B06 -:100DA000730B840BBA0BEE1F88F7E09508950E9431 -:100DB000730788F09F5798F0B92F9927B751B0F073 -:100DC000E1F0660F771F881F991F1AF0BA95C9F7CF -:100DD00014C0B13091F00E948D07B1E008950C94D9 -:100DE0008D07672F782F8827B85F39F0B93FCCF38C -:100DF000869577956795B395D9F73EF4909580954C -:100E0000709561957F4F8F4F9F4F0895E89409C06B -:100E100097FB3EF490958095709561957F4F8F4F2D -:100E20009F4F9923A9F0F92F96E9BB279395F69543 -:100E3000879577956795B795F111F8CFFAF4BB0FC1 -:100E400011F460FF1BC06F5F7F4F8F4F9F4F16C025 -:100E5000882311F096E911C0772321F09EE8872FAF -:100E6000762F05C0662371F096E8862F70E060E06B -:100E70002AF09A95660F771F881FDAF7880F9695E4 -:100E8000879597F9089597F99F6780E870E060E08B -:100E900008959FEF80EC089500240A941616170613 -:100EA00018060906089500240A941216130614065B -:100EB00005060895092E0394000C11F4882352F0BE -:100EC000BB0F40F4BF2B11F460FF04C06F5F7F4F76 -:100ED0008F4F9F4F089557FD9058440F551F59F05D -:100EE0005F3F71F04795880F97FB991F61F09F3F17 -:100EF00079F087950895121613061406551FF2CF40 -:100F00004695F1DF08C0161617061806991FF1CF8F -:100F100086957105610508940895E894BB276627B6 -:100F20007727CB0197F908950E94A7070C945A07D9 -:100F30000E944C0738F00E94530720F0952311F0CF -:100F40000C9443070C94490711240C948E070E94BB -:100F50006B0770F3959FC1F3950F50E0551F629F8B -:100F6000F001729FBB27F00DB11D639FAA27F00D02 -:100F7000B11DAA1F649F6627B00DA11D661F829F29 -:100F80002227B00DA11D621F739FB00DA11D621F0E -:100F9000839FA00D611D221F749F3327A00D611D2B -:100FA000231F849F600D211D822F762F6A2F11240D -:100FB0009F5750409AF0F1F088234AF0EE0FFF1F40 -:100FC000BB1F661F771F881F91505040A9F79E3F97 -:100FD000510580F00C9443070C948E075F3FE4F3B7 -:100FE000983ED4F3869577956795B795F795E795ED -:100FF0009F5FC1F7FE2B880F911D9695879597F9F6 -:101000000895FC010590061621F00020D9F7C001D3 -:1010100008953197CF0108950F931F93CF93DF93D6 -:10102000009731F46115710519F480E090E03AC041 -:101030008B01EC016EE070E081E090E00E9413090A -:10104000FC01892B91F380E883830115110571F070 -:101050001387028781E8838380918D0190918E01AF -:10106000892B21F4F0938E01E0938D012097C9F034 -:10107000D187C08783818260838380918F01909123 -:101080009001892B71F4F0939001E0938F0180918E -:10109000910190919201892B21F4F0939201E093B8 -:1010A0009101CF01DF91CF911F910F9108950F937F -:1010B0001F93CF93DF93FB01238121FD03C08FEFAB -:1010C0009FEF28C022FF16C04681578124813581B9 -:1010D0004217530744F4A081B1819D012F5F3F4F18 -:1010E000318320838C93268137812F5F3F4F378355 -:1010F000268310C0EB01092F182F0084F185E02D05 -:101100000995892BE1F68E819F8101969F838E83BD -:10111000812F902FDF91CF911F910F910895A0E023 -:10112000B0E0E5E9F8E00C94AD05AE01495F5F4F32 -:10113000DA016D917D91AD010DE811E0F801828138 -:101140009381DC0113962C911397286013962C93AE -:101150000E944004D8011296ED91FC9113972381CF -:10116000277F2383E4E00C94C905FA01AA272830DD -:1011700051F1203181F1E8946F936E7F6E5F7F4F64 -:101180008F4F9F4FAF4FB1E03ED0B4E03CD0670FE0 -:10119000781F891F9A1FA11D680F791F8A1F911D33 -:1011A000A11D6A0F711D811D911DA11D20D009F483 -:1011B00068943F912AE0269F11243019305D3193C5 -:1011C000DEF6CF010895462F4770405D4193B3E0AE -:1011D0000FD0C9F7F6CF462F4F70405D4A3318F055 -:1011E000495D31FD4052419302D0A9F7EACFB4E006 -:1011F000A6959795879577956795BA95C9F70097BE -:101200006105710508959B01AC010A2E069457955E -:10121000479537952795BA95C9F7620F731F841FB5 -:10122000951FA01D08950F931F93CF93DF93869F63 -:101230008001879F100D969F100D1124C8010E94F8 -:101240002F09EC01009729F0A80160E070E00E94EE -:101250005C0ACE01DF91CF911F910F910895CF933A -:10126000DF938230910510F482E090E0E0919501E7 -:10127000F091960120E030E0C0E0D0E0309711F12D -:101280004081518148175907C0F04817590761F448 -:1012900082819381209719F09B838A832BC090933E -:1012A00096018093950126C02115310519F042174A -:1012B000530718F49A01BE01DF01EF010280F381A8 -:1012C000E02DDCCF2115310509F1281B390B243025 -:1012D000310590F412968D919C91139761157105CB -:1012E00021F0FB019383828304C090939601809345 -:1012F0009501FD01329644C0FD01E20FF31F819379 -:101300009193225031092D933C933AC0209193013F -:1013100030919401232B41F420910201309103017B -:1013200030939401209393012091000130910101A9 -:101330002115310541F42DB73EB74091040150917C -:101340000501241B350BE0919301F0919401E21704 -:10135000F307A0F42E1B3F0B2817390778F0AC01D8 -:101360004E5F5F4F2417350748F04E0F5F1F5093B5 -:101370009401409393018193919302C0E0E0F0E0E7 -:10138000CF01DF91CF9108950F931F93CF93DF93F8 -:10139000009709F48CC0FC013297138212820091ED -:1013A0009501109196010115110581F4208131817B -:1013B000820F931F209193013091940128173907D0 -:1013C00079F5F0939401E093930171C0D80140E066 -:1013D00050E0AE17BF0750F412962D913C91139731 -:1013E000AD012115310509F1D901F3CF9D01DA01D4 -:1013F0003383228360817181860F971F8217930741 -:1014000069F4EC0128813981260F371F2E5F3F4F89 -:10141000318320838A819B8193838283452B29F4A6 -:10142000F0939601E093950142C01396FC93EE93DE -:101430001297ED01499159919E01240F351FE21732 -:10144000F30771F480819181840F951F02961196A4 -:101450009C938E938281938113969C938E93129783 -:10146000E0E0F0E0D80112968D919C9113970097DF -:1014700019F0F8018C01F6CF8D919C9198012E5FA7 -:101480003F4F820F931F20919301309194012817B1 -:10149000390769F4309729F410929601109295015A -:1014A00002C0138212821093940100939301DF9182 -:1014B000CF911F910F910895DC0101C06D934150B0 -:1014C0005040E0F70895262FF999FECF1FBA92BD3C -:1014D00081BD20BD0FB6F894FA9AF99A0FBE019615 -:0614E0000895F894FFCF0F -:0614E60000009701200048 +:10011000FF938091B900887F803A09F49CC028F44D +:10012000803669F0803889F098C0883B09F44CC06B +:10013000883F09F495C0883A09F08FC045C085EC26 +:100140008093BC00109221018BC0809121018F30DF +:1001500029F18F3F61F181112EC08091BB00809306 +:10016000210180912101853021F481E080931501E6 +:100170000FC080912101873021F481E08093130129 +:1001800007C0809121018B3019F481E08093140124 +:100190008091210181508093210107C08091BB0093 +:1001A000882311F05D9A01C05D9885EC05C08091AF +:1001B000BB008093200185E88093BC0080912101E1 +:1001C0008F3F09F042C04CC0E09121018E2F90E09A +:1001D000FC013197EA31F10598F5EC5CFF4F0C9486 +:1001E000A70580911E0190911F0104C080911C0100 +:1001F00090911D0180930C0190930D019093BB0091 +:100200001DC08091110190911201F4CF80910F01D6 +:1002100090911001EFCF80911A0190911B01EACFCC +:100220008091160190911701E5CF8091180190916E +:100230001901E0CF80910C018093BB0085EC03C0D5 +:100240001092BB0085E88093BC00809121018F5FF4 +:100250008093210105C085EC01C085ED8093BC0031 +:10026000FF91EF91BF91AF919F918F910F900FBE32 +:100270000F901F90189597E890937A0080937C00D8 +:1002800080917C00806480937C0080917A008064FF +:1002900080937A0080917A0086FDFCCF809178006F +:1002A0009091790025E080E090E030917A00306410 +:1002B00030937A0030917A0036FDFCCF409178007F +:1002C00050917900840F951F215079F720917A0081 +:1002D0002F7720937A0065E070E00E947405CB01CF +:1002E00008951F920F920FB60F9211242F933F93F0 +:1002F0004F935F936F937F938F939F93AF93BF932E +:10030000EF93FF9360918400709185004A9B05C034 +:1003100070930B0160930A011DC080910A019091B6 +:100320000B01681B790B80E090E00E94140727EE18 +:100330003CE840E350E40E94A20720E030E040E0C7 +:100340005FE30E9407060E94E50670931B0160931D +:100350001A01E898FF91EF91BF91AF919F918F9112 +:100360007F916F915F914F913F912F910F900FBEB1 +:100370000F901F9018951F920F920FB60F92112495 +:100380002F933F934F935F936F937F938F939F939D +:10039000AF93BF93EF93FF936091840070918500BA +:1003A0004B9B05C070930901609308011DC08091AB +:1003B000080190910901681B790B80E090E00E9490 +:1003C000140727EE3CE840E350E40E94A20720E037 +:1003D00030E040E05FE30E9407060E94E50670936C +:1003E000170160931601E998FF91EF91BF91AF91CA +:1003F0009F918F917F916F915F914F913F912F913D +:100400000F900FBE0F901F9018951F920F920FB66E +:100410000F9211242F933F934F935F936F937F938A +:100420008F939F93AF93BF93EF93FF93CF93DF93FC +:100430001F92CDB7DEB789B1898390910E018927CC +:1004400084FF2DC06091840070918500898184FFB4 +:1004500005C0709307016093060121C080910601D9 +:1004600090910701681B790B80E090E00E941407CF +:1004700027EE3CE840E350E40E94A20720E030E091 +:1004800040E05FE30E9407060E94E50670931901B1 +:100490006093180180916D008F7E80936D0089813B +:1004A00080930E010F90DF91CF91FF91EF91BF915B +:1004B000AF919F918F917F916F915F914F913F91FC +:1004C0002F910F900FBE0F901F9018951F920F92B3 +:1004D0000FB60F9211248F939F93EF93FF93809108 +:1004E000C00087FF17C0E0912401F09125018091A1 +:1004F000C600808381E0E938F80730F43196F09344 +:100500002501E093240106C086E291E09093250145 +:1005100080932401FF91EF919F918F910F900FBED7 +:100520000F901F9018959C01E1ECF0E088E180832A +:1005300080818068808386E08093C200C901A0E04A +:10054000B0E09C01AD0184E0220F331F441F551F12 +:100550008A95D1F760E074E284EF90E00E9488050C +:10056000DA01C9010197A109B109892F9A2FAB2F8F +:10057000BB27A7FDBA958093C50021502093C400E6 +:1005800086E291E0909323018093220190932501CC +:100590008093240108952091C00025FFFCCFFC0129 +:1005A00080818093C6000895CF93DF931F92CDB7CB +:1005B000DEB78983CE0101960E94CB0280E090E0F5 +:1005C0000F90DF91CF910895CF93DF93EC01CE018F +:1005D0002196FC012081222319F00E94CB02F7CF43 +:1005E000DF91CF9108958091C00087FFFCCF80916B +:1005F000C60090E008950C94F3026BEF72E084ED76 +:1006000092E00C941A08CF93DF93FC01E253FF4F62 +:10061000A081B181EC01C453DF4F28813981A21739 +:10062000B307B9F02091C00025FF13C02C9120938F +:10063000C60020813181AC0145535F4F2417350737 +:1006400018F42F5F3F4F03C09C0128593F4F31835F +:100650002083E0912201F091230120912401309127 +:1006600025012E173F07B1F1EC01CC59DF4FA881CE +:10067000B98120812C93688179819C012B523F4F55 +:10068000D9013C91DB012C91321307C0AC014C52D3 +:100690005F4FDA012C912F5F2C939C012D593F4F16 +:1006A0006217730728F4DB011196B983A88302C08F +:1006B00099838883B1E0E938FB0730F43196F093F1 +:1006C0002301E093220106C086E291E0909323018A +:1006D00080932201DF91CF9108951092200180E84C +:1006E00095E20E9493020E94FD0280E28AB91BB843 +:1006F00082E58093BA0085EC8093BC001092800064 +:1007000084E08093810095E0909369008093680075 +:100710008CE990E09F938F930E949D0883B7817F1F +:1007200083BF78940F900F90C8E1D8E080912101A9 +:100730008930E1F068F4813089F0833009F060C0DD +:1007400081E00E943B0190931D0180931C0158C0E1 +:100750008D3009F18F3F09F43FC052C080E00E9404 +:100760003B0190931F0180931E014AC082E00E94CA +:100770003B01BC0180E090E00E94140728E431EEC8 +:100780004AEB5FE30E94A2070E94E5067093120104 +:100790006093110135C083E00E943B01BC0180E001 +:1007A00090E00E94140725E13CE840EA50E40E94F2 +:1007B000A2072CEE31E741E054E40E94060622EF46 +:1007C00038E941E05EE30E9473060E94E5067093FB +:1007D000100160930F0114C080912001853A81F4CB +:1007E000F8946BE780E090E00E94710AF999FECFDF +:1007F0000FB6F894A895C09360000FBED093600028 +:1008000080911501882391F01092150110921B011F +:1008100010921A01529A5A9A85E38A95F1F70000CC +:100820005A9852988CB381708CBBE89A80911301CE +:10083000882391F010921301109217011092160163 +:10084000539A5B9A85E38A95F1F700005B98539879 +:100850008CB382708CBBE99A809114018823B1F02B +:10086000109214011092190110921801549A5C9A76 +:1008700085E38A95F1F700005C98549889B18093DC +:100880000E0180916D00806180936D0083B781605F +:1008900083BF889583B78E7F83BF48CFACE0B0E03D +:1008A000E4E5F4E00C94B1056C015B017A01FC0114 +:1008B00017821682838181FF0EC1CE0101964C0101 +:1008C000F6010381F50103FD159103FF11915F010D +:1008D000112309F4FCC0153249F403FD159103FFFF +:1008E00011915F01153211F020E01BC0B601812F7C +:1008F00090E00E94650837017301E2CF112309F4EB +:10090000E6C0612F70E08DEA90E02C870E940F080E +:100910002C85892B49F0F50103FD159103FF1191F9 +:100920005F0127FFEBCF09C0133229F01C3609F015 +:10093000D5C02068F0CF20E1EECF022F112309F4BB +:10094000C6C0612F70E086EA90E02C870E940F08F5 +:100950002C85892B41F03701F4E06F0E711CB60134 +:100960008FE390E011C0133639F0133781F013355F +:1009700019F5022F01600BC0370182E0680E711C6F +:10098000B601F701808191810E946508B5CF3701DA +:10099000F2E06F0E711CF701E080F180F70100FDBD +:1009A000859100FF81917F01882309F4A5CFB601CD +:1009B00090E00E946508F2CF143611F0193629F53F +:1009C000370127FF09C0F4E06F0E711CF701608149 +:1009D0007181828193810AC0F2E06F0E711CF70170 +:1009E00060817181072E000C880B990B022F0F7EFE +:1009F00097FF08C090958095709561957F4F8F4FB8 +:100A00009F4F00642AE030E033C0103799F040F483 +:100A10001835B1F01F3609F05AC028E030E014C094 +:100A2000153719F0183741F052C0022F0F7E2AE017 +:100A300030E00AC0022F0061046220E130E004C00F +:100A4000022F046020E132E0370107FF09C0F4E023 +:100A50006F0E711CF701608171818281938108C0E2 +:100A6000F2E06F0E711CF7016081718180E090E00F +:100A7000A4010E94C308182F181906FF05C0B6016B +:100A80008DE290E00E94650804FF15C0FE01E10FB1 +:100A9000F11D8081803379F0B60180E390E00E94FF +:100AA000650802FF08C00072802F90E0B601885AE6 +:100AB0009F4F0E9465081150F401E10FF11D8081E4 +:100AC000B60190E00E9465081111F5CF15CFF6012F +:100AD0008681978105C08FEF9FEF02C0022F31CF33 +:100AE0002C96EEE00C94CD05AA1BBB1B51E107C070 +:100AF000AA1FBB1FA617B70710F0A61BB70B881FAE +:100B0000991F5A95A9F780959095BC01CD0108953C +:100B1000052E97FB1EF400940E949F0557FD07D0F9 +:100B20000E94E40507FC03D04EF40C949F055095F9 +:100B30004095309521953F4F4F4F5F4F08959095C9 +:100B40008095709561957F4F8F4F9F4F0895EE0F61 +:100B5000FF1F0590F491E02D09942F923F924F9240 +:100B60005F926F927F928F929F92AF92BF92CF923D +:100B7000DF92EF92FF920F931F93CF93DF93CDB746 +:100B8000DEB7CA1BDB0B0FB6F894DEBF0FBECDBFBE +:100B900009942A88398848885F846E847D848C848F +:100BA0009B84AA84B984C884DF80EE80FD800C8198 +:100BB0001B81AA81B981CE0FD11D0FB6F894DEBF7B +:100BC0000FBECDBFED010895A1E21A2EAA1BBB1BDB +:100BD000FD010DC0AA1FBB1FEE1FFF1FA217B30709 +:100BE000E407F50720F0A21BB30BE40BF50B661F1F +:100BF000771F881F991F1A9469F7609570958095E3 +:100C000090959B01AC01BD01CF0108955058BB27C1 +:100C1000AA270E941E060C9468070E945A0738F003 +:100C20000E94610720F039F49F3F19F426F40C94D8 +:100C300057070EF4E095E7FB0C945107E92F0E944B +:100C4000790758F3BA17620773078407950720F0EE +:100C500079F4A6F50C949B070EF4E0950B2EBA2FB1 +:100C6000A02D0B01B90190010C01CA01A0011124B2 +:100C7000FF27591B99F0593F50F4503E68F11A165E +:100C8000F040A22F232F342F4427585FF3CF4695EF +:100C900037952795A795F0405395C9F77EF41F1611 +:100CA000BA0B620B730B840BBAF09150A1F0FF0FDB +:100CB000BB1F661F771F881FC2F70EC0BA0F621FC7 +:100CC000731F841F48F4879577956795B795F795B7 +:100CD0009E3F08F0B0CF9395880F08F09927EE0F4C +:100CE0009795879508950E9487060C9468070E943F +:100CF000610758F00E945A0740F029F45F3F29F03D +:100D00000C94510751110C949C070C9457070E94A6 +:100D1000790768F39923B1F3552391F3951B550B8C +:100D2000BB27AA2762177307840738F09F5F5F4FBE +:100D3000220F331F441FAA1FA9F335D00E2E3AF0FD +:100D4000E0E832D091505040E695001CCAF72BD015 +:100D5000FE2F29D0660F771F881FBB1F2617370766 +:100D60004807AB07B0E809F0BB0B802DBF01FF2798 +:100D700093585F4F3AF09E3F510578F00C9451071D +:100D80000C949C075F3FE4F3983ED4F386957795E7 +:100D90006795B795F7959F5FC9F7880F911D969551 +:100DA000879597F90895E1E0660F771F881FBB1FAD +:100DB000621773078407BA0720F0621B730B840B5A +:100DC000BA0BEE1F88F7E09508950E94810788F01E +:100DD0009F5798F0B92F9927B751B0F0E1F0660FFF +:100DE000771F881F991F1AF0BA95C9F714C0B13040 +:100DF00091F00E949B07B1E008950C949B07672F28 +:100E0000782F8827B85F39F0B93FCCF3869577956E +:100E10006795B395D9F73EF4909580957095619557 +:100E20007F4F8F4F9F4F0895E89409C097FB3EF482 +:100E300090958095709561957F4F8F4F9F4F992327 +:100E4000A9F0F92F96E9BB279395F69587957795A5 +:100E50006795B795F111F8CFFAF4BB0F11F460FF65 +:100E60001BC06F5F7F4F8F4F9F4F16C0882311F0BD +:100E700096E911C0772321F09EE8872F762F05C0D1 +:100E8000662371F096E8862F70E060E02AF09A956C +:100E9000660F771F881FDAF7880F9695879597F961 +:100EA000089597F99F6780E870E060E008959FEFEC +:100EB00080EC089500240A941616170618060906F1 +:100EC000089500240A9412161306140605060895C0 +:100ED000092E0394000C11F4882352F0BB0F40F448 +:100EE000BF2B11F460FF04C06F5F7F4F8F4F9F4F88 +:100EF000089557FD9058440F551F59F05F3F71F00A +:100F00004795880F97FB991F61F09F3F79F0879570 +:100F10000895121613061406551FF2CF4695F1DFF9 +:100F200008C0161617061806991FF1CF8695710589 +:100F3000610508940895E894BB2766277727CB01BD +:100F400097F908950E94B5070C9468070E945A0704 +:100F500038F00E94610720F0952311F00C9451079E +:100F60000C94570711240C949C070E94790770F386 +:100F7000959FC1F3950F50E0551F629FF001729F3E +:100F8000BB27F00DB11D639FAA27F00DB11DAA1F4D +:100F9000649F6627B00DA11D661F829F2227B00D9A +:100FA000A11D621F739FB00DA11D621F839FA00D25 +:100FB000611D221F749F3327A00D611D231F849F75 +:100FC000600D211D822F762F6A2F11249F575040CC +:100FD0009AF0F1F088234AF0EE0FFF1FBB1F661F47 +:100FE000771F881F91505040A9F79E3F510580F010 +:100FF0000C9451070C949C075F3FE4F3983ED4F3A4 +:10100000869577956795B795F795E7959F5FC1F7B3 +:10101000FE2B880F911D9695879597F90895FC01F1 +:101020000590061621F00020D9F7C00108953197E8 +:10103000CF0108950F931F93CF93DF93009731F45F +:101040006115710519F480E090E03AC08B01EC0164 +:101050006EE070E081E090E00E942109FC01892BA4 +:1010600091F380E883830115110571F013870287DE +:1010700081E8838380918D0190918E01892B21F4E9 +:10108000F0938E01E0938D012097C9F0D187C0873E +:1010900083818260838380918F0190919001892B5D +:1010A00071F4F0939001E0938F0180919101909100 +:1010B0009201892B21F4F0939201E0939101CF01E9 +:1010C000DF91CF911F910F9108950F931F93CF93AD +:1010D000DF93FB01238121FD03C08FEF9FEF28C029 +:1010E00022FF16C04681578124813581421753075C +:1010F00044F4A081B1819D012F5F3F4F3183208354 +:101100008C93268137812F5F3F4F3783268310C012 +:10111000EB01092F182F0084F185E02D0995892B0B +:10112000E1F68E819F8101969F838E83812F902F80 +:10113000DF91CF911F910F910895A0E0B0E0E3EA15 +:10114000F8E00C94BB05AE01495F5F4FDA016D9189 +:101150007D91AD010DE811E0F80182819381DC0100 +:1011600013962C911397286013962C930E944E048B +:10117000D8011296ED91FC9113972381277F238349 +:10118000E4E00C94D705FA01AA27283051F1203168 +:1011900081F1E8946F936E7F6E5F7F4F8F4F9F4F0B +:1011A000AF4FB1E03ED0B4E03CD0670F781F891F4D +:1011B0009A1FA11D680F791F8A1F911DA11D6A0F1B +:1011C000711D811D911DA11D20D009F468943F91CE +:1011D0002AE0269F11243019305D3193DEF6CF01CD +:1011E0000895462F4770405D4193B3E00FD0C9F793 +:1011F000F6CF462F4F70405D4A3318F0495D31FD00 +:101200004052419302D0A9F7EACFB4E0A695979552 +:10121000879577956795BA95C9F700976105710528 +:1012200008959B01AC010A2E069457954795379572 +:101230002795BA95C9F7620F731F841F951FA01DCC +:1012400008950F931F93CF93DF93869F8001879F0D +:10125000100D969F100D1124C8010E943D09EC014C +:10126000009729F0A80160E070E00E946A0ACE01B0 +:10127000DF91CF911F910F910895CF93DF9382302B +:10128000910510F482E090E0E0919501F0919601D3 +:1012900020E030E0C0E0D0E0309711F14081518192 +:1012A00048175907C0F04817590761F482819381A4 +:1012B000209719F09B838A832BC09093960180938B +:1012C000950126C02115310519F04217530718F46E +:1012D0009A01BE01DF01EF010280F381E02DDCCF36 +:1012E0002115310509F1281B390B2430310590F403 +:1012F00012968D919C9113976115710521F0FB0158 +:101300009383828304C09093960180939501FD019D +:10131000329644C0FD01E20FF31F81939193225056 +:1013200031092D933C933AC020919301309194015F +:10133000232B41F420910201309103013093940159 +:101340002093930120910001309101012115310575 +:1013500041F42DB73EB74091040150910501241B83 +:10136000350BE0919301F0919401E217F307A0F49B +:101370002E1B3F0B2817390778F0AC014E5F5F4FEB +:101380002417350748F04E0F5F1F50939401409388 +:1013900093018193919302C0E0E0F0E0CF01DF91EF +:1013A000CF9108950F931F93CF93DF93009709F484 +:1013B0008CC0FC013297138212820091950110912A +:1013C00096010115110581F420813181820F931F4F +:1013D00020919301309194012817390779F5F09302 +:1013E0009401E093930171C0D80140E050E0AE1742 +:1013F000BF0750F412962D913C911397AD01211522 +:10140000310509F1D901F3CF9D01DA01338322833C +:1014100060817181860F971F8217930769F4EC0131 +:1014200028813981260F371F2E5F3F4F318320835C +:101430008A819B8193838283452B29F4F0939601C3 +:10144000E093950142C01396FC93EE931297ED0141 +:10145000499159919E01240F351FE217F30771F44A +:1014600080819181840F951F029611969C938E9393 +:101470008281938113969C938E931297E0E0F0E023 +:10148000D80112968D919C911397009719F0F8014D +:101490008C01F6CF8D919C9198012E5F3F4F820F6A +:1014A000931F20919301309194012817390769F413 +:1014B000309729F4109296011092950102C0138280 +:1014C00012821093940100939301DF91CF911F91A9 +:1014D0000F910895DC0101C06D9341505040E0F739 +:1014E0000895262FF999FECF1FBA92BD81BD20BD68 +:1014F0000FB6F894FA9AF99A0FBE01960895F894E7 +:02150000FFCF1B +:061502000000970120002B :00000001FF diff --git a/scripts/wt_node.py b/scripts/wt_node.py index ee0aa41..03d61be 100755 --- a/scripts/wt_node.py +++ b/scripts/wt_node.py @@ -16,6 +16,7 @@ from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from dynamic_reconfigure.server import Server from sensor_msgs.msg import Imu, Range, BatteryState from wild_thumper.cfg import WildThumperConfig +from std_srvs.srv import SetBool, SetBoolResponse WHEEL_DIST = 0.248 BATTERY_CAPACITY_FULL = 2.750 # Ah, NiMH=2.750, LiFePO4=3.100 @@ -47,6 +48,7 @@ class WTBase: i2c_write_reg(0x50, 0x90, struct.pack("BB", 1, 1)) # switch direction rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived) rospy.Subscriber("imu", Imu, self.imuReceived) + rospy.Service('~manipulator_enable', SetBool, self.enableManipulator) self.bDocked = False self.bDocked_last = False self.battery_capacity = BATTERY_CAPACITY_FULL*3600 # As @@ -373,7 +375,11 @@ class WTBase: rospy.loginfo("Redocking done") else: rospy.logerr("Redocking failed") - + + def enableManipulator(self, msg): + rospy.loginfo("Set Manipulator: %d", msg.data) + i2c_write_reg(0x52, 0x0f, chr(msg.data)) + return SetBoolResponse(True, "") if __name__ == "__main__": WTBase() -- 2.39.2