1 .file "srx1.c" 2 .arch atmega128 3 __SREG__ = 0x3f 4 __SP_H__ = 0x3e 5 __SP_L__ = 0x3d 6 __tmp_reg__ = 0 7 __zero_reg__ = 1 8 .global __do_copy_data 9 .global __do_clear_bss 12 .text 13 .Ltext0: 134 .section .progmem.data,"a",@progbits 137 str_exec_login: 138 0000 0A0D 5352 .string "\n\rSRX1 Executive version 1.0" 138 5831 2045 138 7865 6375 138 7469 7665 138 2076 6572 141 str_exec_login_bar: 142 001d 0A0D 3D3D .string "\n\r=======================================" 142 3D3D 3D3D 142 3D3D 3D3D 142 3D3D 3D3D 142 3D3D 3D3D 145 str_exec_quit: 146 0047 0A0D 5175 .string "\n\rQuitting...\n\r" 146 6974 7469 146 6E67 2E2E 146 2E0A 0D00 149 str_soft_reset: 150 0057 0A0D 5265 .string "\n\rRebooting..." 150 626F 6F74 150 696E 672E 150 2E2E 00 153 str_enter_port_letter: 154 0066 0A0D 456E .string "\n\rEnter port letter: " 154 7465 7220 154 706F 7274 154 206C 6574 154 7465 723A 157 str_port_read_banner: 158 007c 0A0D 504F .string "\n\rPORT" 158 5254 00 161 str_port_read_banner2: 162 0083 0937 3635 .string "\t76543210\n\r\t\t" 162 3433 3231 162 300A 0D09 162 0900 165 str_spacer: 166 0091 0A0D 2D2D .string "\n\r--------------------------------------" 166 2D2D 2D2D 166 2D2D 2D2D 166 2D2D 2D2D 166 2D2D 2D2D 169 str_behaviour_status2: 170 00ba 0A0D 4265 .ascii "\n\rBehaviour\tEnabled\tStatus\tPrior" 170 6861 7669 170 6F75 7209 170 456E 6162 170 6C65 6409 171 00da 6974 7909 .string "ity\tOutput\tType\n\r=========\t=======\t======\t========\t======\t====\n\r" 171 4F75 7470 171 7574 0954 171 7970 650A 171 0D3D 3D3D 174 str_enter_address: 175 011b 0A0D 456E .string "\n\rEnter I2C address: " 175 7465 7220 175 4932 4320 175 6164 6472 175 6573 733A 178 str_enter_element: 179 0131 0A0D 456E .string "\n\rEnter element: " 179 7465 7220 179 656C 656D 179 656E 743A 179 2000 182 str_enter_byte: 183 0143 0A0D 456E .string "\n\rEnter data: " 183 7465 7220 183 6461 7461 183 3A20 00 186 str_line_sensors: 187 0152 0A0D 4C69 .string "\n\rLine Sensors: " 187 6E65 2053 187 656E 736F 187 7273 3A20 187 00 190 str_error: 191 0163 0A0D 4552 .string "\n\rERROR: " 191 524F 523A 191 2000 194 str_cartesian_position: 195 016d 0A0D 4361 .string "\n\rCartesian Position: " 195 7274 6573 195 6961 6E20 195 506F 7369 195 7469 6F6E 198 str_gpd_readings: 199 0184 0A0D 4750 .string "\n\rGP2D12 readings:" 199 3244 3132 199 2072 6561 199 6469 6E67 199 733A 00 202 str_parameter_header: 203 0197 0A0D 4944 .string "\n\rID\t\tParameter\t\t\tValue\n\r--\t\t---------\t\t\t-----" 203 0909 5061 203 7261 6D65 203 7465 7209 203 0909 5661 206 str_parameter_f_p: 207 01c6 0A0D 3031 .string "\n\r01\t\tFollow P coefficient\t\t" 207 0909 466F 207 6C6C 6F77 207 2050 2063 207 6F65 6666 210 str_parameter_f_d: 211 01e3 0A0D 3032 .string "\n\r02\t\tFollow D coefficient\t\t" 211 0909 466F 211 6C6C 6F77 211 2044 2063 211 6F65 6666 214 str_parameter_Kp: 215 0200 0A0D 3033 .string "\n\r03\t\tMotor P coefficient\t\t" 215 0909 4D6F 215 746F 7220 215 5020 636F 215 6566 6669 218 str_parameter_Ki: 219 021c 0A0D 3034 .string "\n\r04\t\tMotor I coefficient\t\t" 219 0909 4D6F 219 746F 7220 219 4920 636F 219 6566 6669 222 str_parameter_Kd: 223 0238 0A0D 3035 .string "\n\r05\t\tMotor D coefficient\t\t" 223 0909 4D6F 223 746F 7220 223 4420 636F 223 6566 6669 226 str_parameter_line_threshold: 227 0254 0A0D 3036 .string "\n\r06\t\tLine Sensor Threshold\t\t" 227 0909 4C69 227 6E65 2053 227 656E 736F 227 7220 5468 230 str_parameter_scm_exposure_h: 231 0272 0A0D 3037 .string "\n\r07\t\tSCM Exposure High\t\t" 231 0909 5343 231 4D20 4578 231 706F 7375 231 7265 2048 234 str_parameter_scm_exposure_l: 235 028c 0A0D 3038 .string "\n\r08\t\tSCM Exposure Low\t\t" 235 0909 5343 235 4D20 4578 235 706F 7375 235 7265 204C 238 str_parameter_scm_threshold: 239 02a5 0A0D 3039 .string "\n\r09\t\tSCM Threshold\t\t\t" 239 0909 5343 239 4D20 5468 239 7265 7368 239 6F6C 6409 242 str_parameter_scm_min_size: 243 02bc 0A0D 3130 .string "\n\r10\t\tSCM Minimum Size Threshold\t" 243 0909 5343 243 4D20 4D69 243 6E69 6D75 243 6D20 5369 246 str_parameter_edit: 247 02de 0A0D 4544 .string "\n\rEDIT: Enter Parameter ID (0 to quit):\n\r" 247 4954 3A20 247 456E 7465 247 7220 5061 247 7261 6D65 250 str_help1: 251 0308 0A0D 0909 .string "\n\r\t\t\t========= Help =========" 251 093D 3D3D 251 3D3D 3D3D 251 3D3D 2048 251 656C 7020 254 str_help2: 255 0326 0A0D 6420 .string "\n\rd - Dump status\t\t\t\tr - Read a port" 255 2D20 4475 255 6D70 2073 255 7461 7475 255 7309 0909 258 str_help3: 259 034b 0A0D 7820 .string "\n\rx - SCM Auto-calibration\t\to - Reset Odometry" 259 2D20 5343 259 4D20 4175 259 746F 2D63 259 616C 6962 262 str_help4: 263 037a 0A0D 5320 .string "\n\rS - Stop motors\t\t\t\tA - Rotate Left" 263 2D20 5374 263 6F70 206D 263 6F74 6F72 263 7309 0909 266 str_help5: 267 039f 0A0D 4420 .string "\n\rD - Rotate Right\t\t\t1 - Forward Low" 267 2D20 526F 267 7461 7465 267 2052 6967 267 6874 0909 270 str_help6: 271 03c4 0A0D 3220 .string "\n\r2 - Forward Medium\t\t\t3 - Forward High" 271 2D20 466F 271 7277 6172 271 6420 4D65 271 6469 756D 274 str_help7: 275 03ec 0A0D 3420 .string "\n\r4 - Backward Low\t\t\t5 - Backward Medium" 275 2D20 4261 275 636B 7761 275 7264 204C 275 6F77 0909 278 str_help8: 279 0415 0A0D 5220 .string "\n\rR - Rotate Abs/Rel\t\t\tF - Travel distance (cm)" 279 2D20 526F 279 7461 7465 279 2041 6273 279 2F52 656C 282 str_help9: 283 0445 0A0D 5420 .string "\n\rT - Follow Wall\t\t\t\tG - Go At coordinate" 283 2D20 466F 283 6C6C 6F77 283 2057 616C 283 6C09 0909 286 str_help10: 287 046f 0A0D 6E20 .string "\n\rn - Align w/ Wall/Door\t\t\th - Go Home from room n" 287 2D20 416C 287 6967 6E20 287 772F 2057 287 616C 6C2F 290 str_help11: 291 04a2 0A0D 6B20 .string "\n\rk - kill PLANNER\t\t\tl - wake up PLANNER" 291 2D20 6B69 291 6C6C 2050 291 4C41 4E4E 291 4552 0909 294 str_help12: 295 04cb 0A0D 6620 .string "\n\rf - Toggle FOLLOW debug\t\t\ta - Toggle ARBITRATE debug" 295 2D20 546F 295 6767 6C65 295 2046 4F4C 295 4C4F 5720 298 str_help13: 299 0502 0A0D 4C20 .string "\n\rL - Reload Parameter Table\t\tI - Re-Init Parameter Table" 299 2D20 5265 299 6C6F 6164 299 2050 6172 299 616D 6574 302 str_help14: 303 053c 0A0D 5020 .string "\n\rP - Dump Parameter Table\t\tE - Edit Parameter Table" 303 2D20 4475 303 6D70 2050 303 6172 616D 303 6574 6572 306 str_help15: 307 0571 0A0D 2120 .string "\n\r! - Software Reset!!!!\t\t\tg - Dump SCM registers" 307 2D20 536F 307 6674 7761 307 7265 2052 307 6573 6574 310 str_exec: 311 05a3 4558 4543 .string "EXEC" 311 00 314 str_avoid: 315 05a8 4156 4F49 .string "AVOID" 315 4400 318 str_forward: 319 05ae 464F 5257 .string "FORWARD" 319 4152 4400 322 str_goat: 323 05b6 474F 5F41 .string "GO_AT" 323 5400 326 str_extinguish: 327 05bc 4558 5449 .string "EXTINGUISH" 327 4E47 5549 327 5348 00 330 str_align: 331 05c7 414C 4947 .string "ALIGN" 331 4E00 334 str_roam: 335 05cd 524F 414D .string "ROAM" 335 00 338 str_rotate: 339 05d2 524F 5441 .string "ROTATE" 339 5445 00 342 str_search: 343 05d9 5345 4152 .string "SEARCH" 343 4348 00 346 str_follow: 347 05e0 464F 4C4C .string "FOLLOW" 347 4F57 00 355 .global execTcb 358 execTcb: 359 05e7 0000 .word execStk+84 360 05e9 0000 .word pm(exec) 361 05eb 0000 .word execPid 362 05ed 04 .byte 4 363 .global arbitrateTcb 366 arbitrateTcb: 367 05ee 0000 .word arbitrateStk+74 368 05f0 0000 .word pm(arbitrate) 369 05f2 0000 .word arbitratePid 370 05f4 02 .byte 2 371 .global followTcb 374 followTcb: 375 05f5 0000 .word followStk+64 376 05f7 0000 .word pm(follow) 377 05f9 0000 .word followPid 378 05fb 05 .byte 5 379 .global goatTcb 382 goatTcb: 383 05fc 0000 .word goatStk+74 384 05fe 0000 .word pm(goat) 385 0600 0000 .word goatPid 386 0602 05 .byte 5 387 .global forwardTcb 390 forwardTcb: 391 0603 0000 .word forwardStk+64 392 0605 0000 .word pm(forward) 393 0607 0000 .word forwardPid 394 0609 05 .byte 5 395 .global rotateTcb 398 rotateTcb: 399 060a 0000 .word rotateStk+64 400 060c 0000 .word pm(rotate) 401 060e 0000 .word rotatePid 402 0610 05 .byte 5 403 .global avoidTcb 406 avoidTcb: 407 0611 0000 .word avoidStk+74 408 0613 0000 .word pm(avoid) 409 0615 0000 .word avoidPid 410 0617 03 .byte 3 411 .global searchTcb 414 searchTcb: 415 0618 0000 .word searchStk+74 416 061a 0000 .word pm(search) 417 061c 0000 .word searchPid 418 061e 05 .byte 5 419 .global extinguishTcb 422 extinguishTcb: 423 061f 0000 .word extinguishStk+64 424 0621 0000 .word pm(extinguish) 425 0623 0000 .word extinguishPid 426 0625 05 .byte 5 427 .global plannerTcb 430 plannerTcb: 431 0626 0000 .word plannerStk+64 432 0628 0000 .word pm(planner) 433 062a 0000 .word plannerPid 434 062c 04 .byte 4 435 .global alignTcb 438 alignTcb: 439 062d 0000 .word alignStk+64 440 062f 0000 .word pm(align) 441 0631 0000 .word alignPid 442 0633 05 .byte 5 443 .global velocity_print 444 .global velocity_print 445 .section .bss 448 velocity_print: 449 0000 00 .skip 1,0 450 .global arbitrate_debug 451 .global arbitrate_debug 454 arbitrate_debug: 455 0001 00 .skip 1,0 456 .global follow_debug 457 .global follow_debug 460 follow_debug: 461 0002 00 .skip 1,0 462 .text 464 .global __vector_16 466 __vector_16: 1:srx1.c **** /************************************************************************************************** 2:srx1.c **** SRX1 robot main cpu 3:srx1.c **** 4:srx1.c **** This robot implements subsumption architecture using Larry Barello's excellent AvrX pre-emptive RT 5:srx1.c **** 6:srx1.c **** See project web page for more details (http://robotics.no-ip.org) 7:srx1.c **** 8:srx1.c **** ToDos (in order of priority) 9:srx1.c **** ===== 10:srx1.c **** - Correct problem with Candle in immediate corner and robot crashing in wall. (enter room deeper b 11:srx1.c **** - Implement a timeout in extinguish so we can abort if both sensors can't agree. 12:srx1.c **** - Enable AVOID for extra robustness??? 13:srx1.c **** - Have the SCM auto-calibrate right after motion sensor triggers and base is stopped??? In case of 14:srx1.c **** - Implement basic filtering of motion sensor input (average samples)???? 15:srx1.c **** - Implement a way to replace hard-coded distance commands in PLANNER. (ex: Look for presence/absen 16:srx1.c **** - For case #1 (Enter Room #1): Simply Follow right wall 'till the end. 17:srx1.c **** OR 18:srx1.c **** Follow Right until front sensor reads around 46cm. 19:srx1.c **** - For case #2 (To room #3): Go forward until both Left sensors are above threshold. 20:srx1.c **** - For case #3 (Enter Room #4): Add a "Travel for x distance" to Follow. 21:srx1.c **** - Have ALIGN align take over the responsibility of aligning with candle??? 22:srx1.c **** - Modify AVOID to better fit new navigational concepts. ex. backup on front threshold, etc. 23:srx1.c **** - Optimize behaviours so they don't keep sending message every time through the loop unecessarily 24:srx1.c **** 25:srx1.c **** ToTest 26:srx1.c **** ====== 27:srx1.c **** - Minimize stack sizes. 28:srx1.c **** 29:srx1.c **** History: 30:srx1.c **** ======= 31:srx1.c **** - August 25th 2002 / Stephane Gauthier / Genesis 32:srx1.c **** - August 26th 2002 / Stephane Gauthier / Got basic PWM working. 33:srx1.c **** - August 28th 2002 / Stephane Gauthier / Added interrupts handlers for ext. int #2 & 3 (left and r 34:srx1.c **** - September 2nd 2002 / Stephane Gauthier / modified encoder ISRs to decrease count when going in r 35:srx1.c **** - December 21st 2002 / Stephane Gauthier / setup OC2 PWM to replace dead OC1A PWM pin (transparent 36:srx1.c **** - January 3rt 2003 / Stephane Gauthier / Total redesign of the behaviour communication system and 37:srx1.c **** - January 12th 2003 / Stephane Gauthier / Ported back to STK-300 board with atmega128. 38:srx1.c **** - January 13th 2003 / Stephane Gauthier / Implementated handling of dynamic priorities in ARBITRAT 39:srx1.c **** - January 14th 2003 / Stephane Gauthier / Optimized AVOID so it will only send message to arbitrat 40:srx1.c **** - January 15th 2003 / Stephane Gauthier / Cleaned up EXEC commands a bit. Added Fan command. 41:srx1.c **** - January 18th 2003 / Stephane Gauthier / Implemented full quadrature decoding of encoders using a 42:srx1.c **** - January 21st 2003 / Stephane Gauthier / Implemented a radically faster quadrature polling algori 43:srx1.c **** - February 5th 2003 / Stephane Gauthier / Modified gpd_read() so it returns the average reading of 44:srx1.c **** - February 8th 2003 / Stephane Gauthier / Redesign basic wall following code. Added door frame det 45:srx1.c **** - February 9th 2003 / Stephane Gauthier / Implemented a way for ROTATE to pass full resolution com 46:srx1.c **** - February 9th 2003 / Stephane Gauthier / Implemented SEARCH. 47:srx1.c **** - February 9th 2003 / Stephane Gauthier / Implemented PLANNER. 48:srx1.c **** - February 10th 2003 / Stephane Gauthier / Implemented a way for PLANNER to enable/disable behavio 49:srx1.c **** - February 10th 2003 / Stephane Gauthier / Fixed/tweaked a few more bugs. Found AVOID to be steal 50:srx1.c **** - February 20th 2003 / Stephane Gauthier / Added a "kill" PLANNER and a FOLLOW/ARBITRATE debug out 51:srx1.c **** - February 24th 2003 / Stephane Gauthier / Tentative Re-design of the navigational stuff to make i 52:srx1.c **** - February 25th 2003 / Stephane Gauthier / Implemented ALIGN: a subset of FOLLOW to staticly align 53:srx1.c **** - February 25th 2003 / Stephane Gauthier / Added status reporting to behaviours so PLANNER can imp 54:srx1.c **** - February 26th 2003 / Stephane Gauthier / Added WaitForMotionToStop(). In ROTATE: fully stop befo 55:srx1.c **** - February 27th 2003 / Stephane Gauthier / Re-organized PLANNER. Moved redundant code to individua 56:srx1.c **** - March 3rd 2003 / Stephane Gauthier / Implemented Parameter Table in EEPROM with associated funct 57:srx1.c **** - March 4th 2003 / Stephane Gauthier / Implemented EmergencyStop() to prevent PID from decellerati 58:srx1.c **** - March 4th 2003 / Stephane Gauthier / Tweaked EXTINGUISH behaviour to track candle as it moves fo 59:srx1.c **** - March 5th 2003 / Stephane Gauthier / Tweaked EXTINGUISH further. Added SCM auto-calibration opti 60:srx1.c **** - March 7th 2003 / Stephane Gauthier / Moved SCM calibration start code to seperate SCM_calibrate( 61:srx1.c **** - March 8th 2003 / Stephane Gauthier / Finished GO_AT implementation (not the best but good enough 62:srx1.c **** - March 8th 2003 / Stephane Gauthier / Refined GO_AT implementation to dynamically main bearing (i 63:srx1.c **** - March 12th 2003 / Stephane Gauthier / Implemented OdometryReset() to MotorTask.c. Added 'Go Home 64:srx1.c **** - March 17th 2003 / Stephane Gauthier / ALIGN now clears the door automatically when asked to alig 65:srx1.c **** - March 18th 2003 / Stephane Gauthier / Redone ALIGN to use simpler centered sensors using a globa 66:srx1.c **** - March 28th 2003 / Stephane Gauthier / Added code to permit Line sensor calibration at startup (v 67:srx1.c **** - April 3rd 2003 / Stephane Gauthier / Commented out the align with door code in PLANNER as it's f 68:srx1.c **** - April 4th 2003 / Stephane Gauthier / Added a "Ignore Door" argument to Forward. Modified Forward 69:srx1.c **** - April 4th 2003 / Stephane Gauthier / Added a call to ALIGN (w/ wall) in SEARCH to compensate for 70:srx1.c **** - April 5th 2003 / Stephane Gauthier / Added a "Ignore Door" argument to Follow. Modified Follow() 71:srx1.c **** - April 5th 2003 / Stephane Gauthier / Test runs at Ron's place: Several minor bug fixes and tweak 72:srx1.c **** - April 7th 2003 / Stephane Gauthier / Have both SCM and motion sensor scan the room in SEARCH for 73:srx1.c **** - April 8th 2003 / Stephane Gauthier / Added code to EXTINGUISH to make it more redundant (rotate 74:srx1.c **** ************************************************************************************************** 75:srx1.c **** #define ENABLE_BIT_DEFINITIONS 76:srx1.c **** //#include 77:srx1.c **** //#include 78:srx1.c **** #include 79:srx1.c **** #include // for printf 80:srx1.c **** #include 81:srx1.c **** #include // for abs() 82:srx1.c **** #include 83:srx1.c **** #include 84:srx1.c **** #include 85:srx1.c **** 86:srx1.c **** #include "c:/avrx/avrx/avrx.h" 87:srx1.c **** #include "hardware.h" 88:srx1.c **** #include "I2C.h" 89:srx1.c **** #include "serialio.h" 90:srx1.c **** #include "generalio.h" 91:srx1.c **** #include "HW_drivers.h" 92:srx1.c **** #include "text_strings.h" 93:srx1.c **** #include "MotorTask.h" 94:srx1.c **** #include "scm\scm_cmds.h" 95:srx1.c **** 96:srx1.c **** // Define all AVRX task here so that all task variables (i.e. Pid, Tcb, etc..) are available in fil 97:srx1.c **** AVRX_GCC_TASKDEF(exec, 50, EXEC_PRIORITY); 98:srx1.c **** AVRX_GCC_TASKDEF(arbitrate, 40, ARBITRATE_PRIORITY); 99:srx1.c **** AVRX_GCC_TASKDEF(follow, 30, FOLLOW_PRIORITY); 100:srx1.c **** AVRX_GCC_TASKDEF(goat, 40, GOAT_PRIORITY); 101:srx1.c **** AVRX_GCC_TASKDEF(forward, 30, FORWARD_PRIORITY); 102:srx1.c **** AVRX_GCC_TASKDEF(rotate, 30, ROTATE_PRIORITY); 103:srx1.c **** AVRX_GCC_TASKDEF(avoid, 40, AVOID_PRIORITY); 104:srx1.c **** AVRX_GCC_TASKDEF(search, 40, SEARCH_PRIORITY); 105:srx1.c **** AVRX_GCC_TASKDEF(extinguish, 30, EXTINGUISH_PRIORITY); 106:srx1.c **** AVRX_GCC_TASKDEF(planner, 30, PLANNER_PRIORITY); 107:srx1.c **** AVRX_GCC_TASKDEF(align, 30, ALIGN_PRIORITY); 108:srx1.c **** AVRX_EXTERNTASK(MotorTask); 109:srx1.c **** 110:srx1.c **** // 111:srx1.c **** // Prototypes 112:srx1.c **** // 113:srx1.c **** void InitBehaviourTable(void); 114:srx1.c **** unsigned char Align( unsigned char type ); 115:srx1.c **** unsigned char Forward( unsigned char lenght, unsigned char expected); 116:srx1.c **** void Rotate( signed int angle, unsigned char scan ); 117:srx1.c **** unsigned char Search(unsigned char side); 118:srx1.c **** unsigned char Follow(unsigned char side, unsigned char expected); 119:srx1.c **** void GoHome(void); 120:srx1.c **** void InitParameterTable(void); 121:srx1.c **** void LoadParameterTable(void); 122:srx1.c **** void DumpParameterTable(void); 123:srx1.c **** void EditParameterTable(unsigned char id); 124:srx1.c **** 125:srx1.c **** // 126:srx1.c **** // Globals 127:srx1.c **** // 128:srx1.c **** 129:srx1.c **** // Imported Odometry Variables 130:srx1.c **** extern volatile int iX, iY, iTheta; // Imported from MotorTask.c 131:srx1.c **** 132:srx1.c **** // Declare the control blocks needed for GLOBAL timers (LOCAL ONES DON'T SEEM TO WANT TO WORK AT AL 133:srx1.c **** AVRX_TIMER(exec_timer); 134:srx1.c **** AVRX_TIMER(goat_timer); 135:srx1.c **** AVRX_TIMER(follow_timer); 136:srx1.c **** AVRX_TIMER(avoid_timer); 137:srx1.c **** AVRX_TIMER(extinguish_timer); 138:srx1.c **** AVRX_TIMER(planner_timer); 139:srx1.c **** AVRX_TIMER(align_timer); 140:srx1.c **** 141:srx1.c **** // Declare semaphores. 142:srx1.c **** //AVRX_MUTEX(rotate_mutex); 143:srx1.c **** //AVRX_MUTEX(forward_mutex); 144:srx1.c **** 145:srx1.c **** // EXEC Debug options 146:srx1.c **** volatile char velocity_print = FALSE; 147:srx1.c **** volatile char arbitrate_debug = FALSE; 148:srx1.c **** volatile char follow_debug = FALSE; 149:srx1.c **** 150:srx1.c **** // Misc Globals 151:srx1.c **** unsigned char Room; // Current Room number 152:srx1.c **** unsigned char last_line_trigger; // Last triggered line sensors 153:srx1.c **** 154:srx1.c **** // Others 155:srx1.c **** ConfigData E_ParameterTable EEPROM; // Non-Volatile storage copy. (defaults) 156:srx1.c **** ConfigData R_ParameterTable; // Runtime copy in SRAM 157:srx1.c **** 158:srx1.c **** // Subsumption variables 159:srx1.c **** BehaviourInfo BehaviourTable[BEHAVIOUR_COUNT]; 160:srx1.c **** MessageQueue RotateQueue; // The Rotate message queue 161:srx1.c **** MessageQueue ForwardQueue; // The Forward message queue 162:srx1.c **** MessageQueue FollowQueue; // The Forward message queue 163:srx1.c **** MessageQueue ArbitrateQueue; // The Arbritrator message queue 164:srx1.c **** MessageQueue GoAtQueue; // The GoAt message queue 165:srx1.c **** MessageQueue ExtinguishQueue; // The Extinguish message queue 166:srx1.c **** MessageQueue AvoidQueue; // The Avoid message queue 167:srx1.c **** MessageQueue SearchQueue; // The Search message queue 168:srx1.c **** MessageQueue AlignQueue; // The Align message queue 169:srx1.c **** 170:srx1.c **** /****************************************** 171:srx1.c **** Timer 0 Overflow Interrupt Handler 172:srx1.c **** 173:srx1.c **** Prototypical Interrupt handler: 174:srx1.c **** . Switch to kernel context 175:srx1.c **** . handle interrupt 176:srx1.c **** . Update Quadrature Encoders counts 177:srx1.c **** . switch back to interrupted context. 178:srx1.c **** ******************************************/ 179:srx1.c **** AVRX_SIGINT(SIG_OVERFLOW0) 180:srx1.c **** { 468 .LM1: 469 /* prologue: frame size=0 */ 470 /* prologue: naked */ 471 /* prologue end (size=0) */ 181:srx1.c **** IntProlog(); // Switch to kernel stack/context 473 .LM2: 474 0000 0E94 0000 call IntProlog 182:srx1.c **** 183:srx1.c **** TCNT0 = TCNT0_INIT; 476 .LM3: 477 0004 86EF ldi r24,lo8(-10) 478 0006 82BF out 82-0x20,r24 184:srx1.c **** 185:srx1.c **** AvrXTimerHandler(); // Call Time queue manager 480 .LM4: 481 0008 0E94 0000 call AvrXTimerHandler 186:srx1.c **** 187:srx1.c **** QuadratureDecoder(); // Update encoder counts 483 .LM5: 484 000c 0E94 0000 call QuadratureDecoder 188:srx1.c **** 189:srx1.c **** // Motion Sensor debug 190:srx1.c **** if (bit_test(MOTION_SENSE)) 486 .LM6: 487 0010 0B9B sbis 33-0x20,3 488 0012 02C0 rjmp .L2 191:srx1.c **** bit_set(LED1); 490 .LM7: 491 0014 C19A sbi 56-0x20,1 492 0016 01C0 rjmp .L3 493 .L2: 192:srx1.c **** else 193:srx1.c **** bit_clear(LED1); 495 .LM8: 496 0018 C198 cbi 56-0x20,1 497 .L3: 194:srx1.c **** 195:srx1.c **** Epilog(); // Return to tasks 499 .LM9: 500 001a 0E94 0000 call Epilog 196:srx1.c **** } 502 .LM10: 503 /* epilogue: frame size=0 */ 504 /* epilogue: naked */ 505 /* epilogue end (size=0) */ 506 /* function __vector_16 size 15 (15) */ 508 .Lscope0: 510 .lcomm RotateCmd.0,9 511 .lcomm ForwardCmd.1,8 512 .lcomm FollowCmd.2,7 513 .lcomm ArbitrateCmd.3,14 514 .lcomm GoAtCmd.4,10 515 .lcomm ExtinguishCmd.5,5 516 .lcomm SearchCmd.6,6 517 .lcomm AlignCmd.7,6 519 .global exec 521 exec: 197:srx1.c **** 198:srx1.c **** 199:srx1.c **** /****************************************** 200:srx1.c **** Task - exec 201:srx1.c **** ******************************************/ 202:srx1.c **** NAKEDFUNC(exec) 203:srx1.c **** { 523 .LM11: 524 /* prologue: frame size=0 */ 525 /* prologue: naked */ 526 /* prologue end (size=0) */ 204:srx1.c **** unsigned char exit = FALSE; 528 .LM12: 529 .LBB2: 530 001e FF24 clr r15 205:srx1.c **** int temp_int; 206:srx1.c **** unsigned char temp_char; 207:srx1.c **** unsigned char temp_char2; 208:srx1.c **** unsigned char index; 209:srx1.c **** static RotateMessage RotateCmd; 210:srx1.c **** static ForwardMessage ForwardCmd; 211:srx1.c **** static FollowMessage FollowCmd; 212:srx1.c **** static ArbitrateMessage ArbitrateCmd; 213:srx1.c **** static GoAtMessage GoAtCmd; 214:srx1.c **** static ExtinguishMessage ExtinguishCmd; 215:srx1.c **** static SearchMessage SearchCmd; 216:srx1.c **** static AlignMessage AlignCmd; 217:srx1.c **** 218:srx1.c **** // Reduce code by initializing it once only. 219:srx1.c **** ArbitrateCmd.id=EXEC; 532 .LM13: 533 0020 F092 0000 sts ArbitrateCmd.3+4,r15 220:srx1.c **** ArbitrateCmd.info.type = ABSOLUTE; // assuming EXEC doesn't use relative PID commands. 535 .LM14: 536 0024 81E0 ldi r24,lo8(1) 537 0026 8093 0000 sts ArbitrateCmd.3+13,r24 221:srx1.c **** 222:srx1.c **** while (1) 223:srx1.c **** { 224:srx1.c **** // Wait for attention code 225:srx1.c **** while (GetChar() != EXEC_KEY) 539 .LM15: 540 .L183: 541 002a 0E94 0000 call GetChar 542 002e 8036 cpi r24,lo8(96) 543 0030 E1F7 brne .L183 226:srx1.c **** ; 227:srx1.c **** 228:srx1.c **** // Suspend tasks to prevent contention issues. (keep arbitrate task running though) 229:srx1.c **** //AvrXSuspend(PID(MotorTask)); 230:srx1.c **** //AvrXSuspend(PID(follow)); 231:srx1.c **** 232:srx1.c **** // Do we need AVOID to back us up here? Not really. 233:srx1.c **** // keep in mind that EXEC doesn't reset it to its initial value on exit! 234:srx1.c **** //BehaviourTable[AVOID].enable = FALSE; 235:srx1.c **** 236:srx1.c **** // Get Sumbsumption Priority 237:srx1.c **** ArbitrateCmd.info.output_l = POWER(0); 545 .LM16: 546 0032 1092 0000 sts (ArbitrateCmd.3+8)+1,__zero_reg__ 547 0036 1092 0000 sts ArbitrateCmd.3+8,__zero_reg__ 238:srx1.c **** ArbitrateCmd.info.output_r = POWER(0); 549 .LM17: 550 003a 1092 0000 sts (ArbitrateCmd.3+10)+1,__zero_reg__ 551 003e 1092 0000 sts ArbitrateCmd.3+10,__zero_reg__ 239:srx1.c **** ArbitrateCmd.info.active = TRUE; 553 .LM18: 554 0042 81E0 ldi r24,lo8(1) 555 0044 8093 0000 sts ArbitrateCmd.3+6,r24 240:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 557 .LM19: 558 0048 60E0 ldi r22,lo8(ArbitrateCmd.3) 559 004a 70E0 ldi r23,hi8(ArbitrateCmd.3) 560 004c 80E0 ldi r24,lo8(ArbitrateQueue) 561 004e 90E0 ldi r25,hi8(ArbitrateQueue) 562 0050 0E94 0000 call AvrXSendMessage 241:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 564 .LM20: 565 0054 80E0 ldi r24,lo8(ArbitrateCmd.3) 566 0056 90E0 ldi r25,hi8(ArbitrateCmd.3) 567 0058 0E94 0000 call AvrXWaitMessageAck 242:srx1.c **** 243:srx1.c **** // Disable printing of velocity by MotorTask 244:srx1.c **** velocity_print = FALSE; 569 .LM21: 570 005c 1092 0000 sts velocity_print,__zero_reg__ 245:srx1.c **** 246:srx1.c **** // Display Welcome Message 247:srx1.c **** PrintString(str_exec_login); 572 .LM22: 573 0060 80E0 ldi r24,lo8(str_exec_login) 574 0062 90E0 ldi r25,hi8(str_exec_login) 575 0064 0E94 0000 call PrintString 248:srx1.c **** PrintString(str_exec_login_bar); 577 .LM23: 578 0068 80E0 ldi r24,lo8(str_exec_login_bar) 579 006a 90E0 ldi r25,hi8(str_exec_login_bar) 580 006c 0E94 0000 call PrintString 581 .L177: 249:srx1.c **** 250:srx1.c **** while (exit == FALSE) 251:srx1.c **** { 252:srx1.c **** PutCRLF(); 583 .LM24: 584 0070 0E94 0000 call PutCRLF 253:srx1.c **** PutChar('>'); // Print EXEC prompt 586 .LM25: 587 0074 8EE3 ldi r24,lo8(62) 588 0076 0E94 0000 call PutChar 254:srx1.c **** 255:srx1.c **** switch (_GetChar()) // Get Command (with echo) 590 .LM26: 591 007a 0E94 0000 call _GetChar 592 007e 9927 clr r25 593 0080 AA27 clr r26 594 0082 BB27 clr r27 595 0084 FC01 movw r30,r24 596 0086 B197 sbiw r30,33 597 0088 E835 cpi r30,88 598 008a F105 cpc r31,__zero_reg__ 599 008c 10F0 brlo .+4 600 008e 0C94 0000 jmp .L11 601 0092 E050 subi r30,lo8(-(pm(.L176))) 602 0094 F040 sbci r31,hi8(-(pm(.L176))) 603 0096 EE0F lsl r30 604 0098 FF1F rol r31 605 009a 0590 lpm __tmp_reg__,Z+ 606 009c F491 lpm r31,Z 607 009e E02D mov r30,__tmp_reg__ 608 00a0 0994 ijmp 609 .data 610 .section .progmem.gcc_sw_table, "a", @progbits 611 .p2align 1 612 .L176: 613 0000 0000 .word pm(.L109) 614 0002 0000 .word pm(.L11) 615 0004 0000 .word pm(.L110) 616 0006 0000 .word pm(.L111) 617 0008 0000 .word pm(.L112) 618 000a 0000 .word pm(.L11) 619 000c 0000 .word pm(.L11) 620 000e 0000 .word pm(.L11) 621 0010 0000 .word pm(.L11) 622 0012 0000 .word pm(.L11) 623 0014 0000 .word pm(.L11) 624 0016 0000 .word pm(.L11) 625 0018 0000 .word pm(.L11) 626 001a 0000 .word pm(.L11) 627 001c 0000 .word pm(.L11) 628 001e 0000 .word pm(.L11) 629 0020 0000 .word pm(.L88) 630 0022 0000 .word pm(.L87) 631 0024 0000 .word pm(.L86) 632 0026 0000 .word pm(.L90) 633 0028 0000 .word pm(.L89) 634 002a 0000 .word pm(.L11) 635 002c 0000 .word pm(.L11) 636 002e 0000 .word pm(.L11) 637 0030 0000 .word pm(.L11) 638 0032 0000 .word pm(.L153) 639 0034 0000 .word pm(.L11) 640 0036 0000 .word pm(.L11) 641 0038 0000 .word pm(.L11) 642 003a 0000 .word pm(.L11) 643 003c 0000 .word pm(.L15) 644 003e 0000 .word pm(.L11) 645 0040 0000 .word pm(.L85) 646 0042 0000 .word pm(.L11) 647 0044 0000 .word pm(.L11) 648 0046 0000 .word pm(.L84) 649 0048 0000 .word pm(.L169) 650 004a 0000 .word pm(.L122) 651 004c 0000 .word pm(.L141) 652 004e 0000 .word pm(.L11) 653 0050 0000 .word pm(.L166) 654 0052 0000 .word pm(.L11) 655 0054 0000 .word pm(.L11) 656 0056 0000 .word pm(.L167) 657 0058 0000 .word pm(.L11) 658 005a 0000 .word pm(.L11) 659 005c 0000 .word pm(.L11) 660 005e 0000 .word pm(.L168) 661 0060 0000 .word pm(.L11) 662 0062 0000 .word pm(.L114) 663 0064 0000 .word pm(.L83) 664 0066 0000 .word pm(.L123) 665 0068 0000 .word pm(.L11) 666 006a 0000 .word pm(.L11) 667 006c 0000 .word pm(.L11) 668 006e 0000 .word pm(.L11) 669 0070 0000 .word pm(.L11) 670 0072 0000 .word pm(.L11) 671 0074 0000 .word pm(.L11) 672 0076 0000 .word pm(.L11) 673 0078 0000 .word pm(.L11) 674 007a 0000 .word pm(.L113) 675 007c 0000 .word pm(.L11) 676 007e 0000 .word pm(.L108) 677 0080 0000 .word pm(.L158) 678 0082 0000 .word pm(.L11) 679 0084 0000 .word pm(.L142) 680 0086 0000 .word pm(.L16) 681 0088 0000 .word pm(.L11) 682 008a 0000 .word pm(.L158) 683 008c 0000 .word pm(.L160) 684 008e 0000 .word pm(.L173) 685 0090 0000 .word pm(.L11) 686 0092 0000 .word pm(.L11) 687 0094 0000 .word pm(.L170) 688 0096 0000 .word pm(.L171) 689 0098 0000 .word pm(.L11) 690 009a 0000 .word pm(.L131) 691 009c 0000 .word pm(.L174) 692 009e 0000 .word pm(.L11) 693 00a0 0000 .word pm(.L11) 694 00a2 0000 .word pm(.L91) 695 00a4 0000 .word pm(.L11) 696 00a6 0000 .word pm(.L11) 697 00a8 0000 .word pm(.L11) 698 00aa 0000 .word pm(.L11) 699 00ac 0000 .word pm(.L11) 700 00ae 0000 .word pm(.L172) 701 .text 702 .L15: 256:srx1.c **** { 257:srx1.c **** // Print Help screen 258:srx1.c **** case ('?'): 259:srx1.c **** PrintString(str_help1); 704 .LM27: 705 00a2 80E0 ldi r24,lo8(str_help1) 706 00a4 90E0 ldi r25,hi8(str_help1) 707 00a6 0E94 0000 call PrintString 260:srx1.c **** PrintString(str_help2); 709 .LM28: 710 00aa 80E0 ldi r24,lo8(str_help2) 711 00ac 90E0 ldi r25,hi8(str_help2) 712 00ae 0E94 0000 call PrintString 261:srx1.c **** PrintString(str_help3); 714 .LM29: 715 00b2 80E0 ldi r24,lo8(str_help3) 716 00b4 90E0 ldi r25,hi8(str_help3) 717 00b6 0E94 0000 call PrintString 262:srx1.c **** PrintString(str_help4); 719 .LM30: 720 00ba 80E0 ldi r24,lo8(str_help4) 721 00bc 90E0 ldi r25,hi8(str_help4) 722 00be 0E94 0000 call PrintString 263:srx1.c **** PrintString(str_help5); 724 .LM31: 725 00c2 80E0 ldi r24,lo8(str_help5) 726 00c4 90E0 ldi r25,hi8(str_help5) 727 00c6 0E94 0000 call PrintString 264:srx1.c **** PrintString(str_help6); 729 .LM32: 730 00ca 80E0 ldi r24,lo8(str_help6) 731 00cc 90E0 ldi r25,hi8(str_help6) 732 00ce 0E94 0000 call PrintString 265:srx1.c **** PrintString(str_help7); 734 .LM33: 735 00d2 80E0 ldi r24,lo8(str_help7) 736 00d4 90E0 ldi r25,hi8(str_help7) 737 00d6 0E94 0000 call PrintString 266:srx1.c **** PrintString(str_help8); 739 .LM34: 740 00da 80E0 ldi r24,lo8(str_help8) 741 00dc 90E0 ldi r25,hi8(str_help8) 742 00de 0E94 0000 call PrintString 267:srx1.c **** PrintString(str_help9); 744 .LM35: 745 00e2 80E0 ldi r24,lo8(str_help9) 746 00e4 90E0 ldi r25,hi8(str_help9) 747 00e6 0E94 0000 call PrintString 268:srx1.c **** PrintString(str_help10); 749 .LM36: 750 00ea 80E0 ldi r24,lo8(str_help10) 751 00ec 90E0 ldi r25,hi8(str_help10) 752 00ee 0E94 0000 call PrintString 269:srx1.c **** PrintString(str_help11); 754 .LM37: 755 00f2 80E0 ldi r24,lo8(str_help11) 756 00f4 90E0 ldi r25,hi8(str_help11) 757 00f6 0E94 0000 call PrintString 270:srx1.c **** PrintString(str_help12); 759 .LM38: 760 00fa 80E0 ldi r24,lo8(str_help12) 761 00fc 90E0 ldi r25,hi8(str_help12) 762 00fe 0E94 0000 call PrintString 271:srx1.c **** PrintString(str_help13); 764 .LM39: 765 0102 80E0 ldi r24,lo8(str_help13) 766 0104 90E0 ldi r25,hi8(str_help13) 767 0106 0E94 0000 call PrintString 272:srx1.c **** PrintString(str_help14); 769 .LM40: 770 010a 80E0 ldi r24,lo8(str_help14) 771 010c 90E0 ldi r25,hi8(str_help14) 772 010e 0E94 0000 call PrintString 273:srx1.c **** PrintString(str_help15); 774 .LM41: 775 0112 80E0 ldi r24,lo8(str_help15) 776 0114 90E0 ldi r25,hi8(str_help15) 777 0116 36C5 rjmp .L223 778 .L16: 274:srx1.c **** break; 275:srx1.c **** 276:srx1.c **** // Dump status 277:srx1.c **** case ('d'): 278:srx1.c **** PrintString(str_exec_login_bar); 780 .LM42: 781 0118 80E0 ldi r24,lo8(str_exec_login_bar) 782 011a 90E0 ldi r25,hi8(str_exec_login_bar) 783 011c 0E94 0000 call PrintString 279:srx1.c **** 280:srx1.c **** // Print current robot position. 281:srx1.c **** PrintString(str_cartesian_position); 785 .LM43: 786 0120 80E0 ldi r24,lo8(str_cartesian_position) 787 0122 90E0 ldi r25,hi8(str_cartesian_position) 788 0124 0E94 0000 call PrintString 282:srx1.c **** PutChar('X'); 790 .LM44: 791 0128 88E5 ldi r24,lo8(88) 792 012a 0E94 0000 call PutChar 283:srx1.c **** PutChar('='); 794 .LM45: 795 012e 8DE3 ldi r24,lo8(61) 796 0130 0E94 0000 call PutChar 284:srx1.c **** PutDecSignedWord(iX); 798 .LM46: 799 0134 8091 0000 lds r24,iX 800 0138 9091 0000 lds r25,(iX)+1 801 013c 0E94 0000 call PutDecSignedWord 285:srx1.c **** PutChar(' '); 803 .LM47: 804 0140 80E2 ldi r24,lo8(32) 805 0142 0E94 0000 call PutChar 286:srx1.c **** PutChar('Y'); 807 .LM48: 808 0146 89E5 ldi r24,lo8(89) 809 0148 0E94 0000 call PutChar 287:srx1.c **** PutChar('='); 811 .LM49: 812 014c 8DE3 ldi r24,lo8(61) 813 014e 0E94 0000 call PutChar 288:srx1.c **** PutDecSignedWord(iY); 815 .LM50: 816 0152 8091 0000 lds r24,iY 817 0156 9091 0000 lds r25,(iY)+1 818 015a 0E94 0000 call PutDecSignedWord 289:srx1.c **** PutChar(' '); 820 .LM51: 821 015e 80E2 ldi r24,lo8(32) 822 0160 0E94 0000 call PutChar 290:srx1.c **** PutChar('@'); 824 .LM52: 825 0164 80E4 ldi r24,lo8(64) 826 0166 0E94 0000 call PutChar 291:srx1.c **** PutChar('='); 828 .LM53: 829 016a 8DE3 ldi r24,lo8(61) 830 016c 0E94 0000 call PutChar 292:srx1.c **** PutDecSignedWord(iTheta); 832 .LM54: 833 0170 8091 0000 lds r24,iTheta 834 0174 9091 0000 lds r25,(iTheta)+1 835 0178 0E94 0000 call PutDecSignedWord 293:srx1.c **** 294:srx1.c **** 295:srx1.c **** // Display Current Active Behaviour 296:srx1.c **** PrintString(str_spacer); 837 .LM55: 838 017c 80E0 ldi r24,lo8(str_spacer) 839 017e 90E0 ldi r25,hi8(str_spacer) 840 0180 0E94 0000 call PrintString 297:srx1.c **** PrintString(str_behaviour_status2); 842 .LM56: 843 0184 80E0 ldi r24,lo8(str_behaviour_status2) 844 0186 90E0 ldi r25,hi8(str_behaviour_status2) 845 0188 0E94 0000 call PrintString 298:srx1.c **** 299:srx1.c **** PrintString(str_exec); // EXEC 847 .LM57: 848 018c 80E0 ldi r24,lo8(str_exec) 849 018e 90E0 ldi r25,hi8(str_exec) 850 0190 0E94 0000 call PrintString 300:srx1.c **** PutChar('\t'); 852 .LM58: 853 0194 89E0 ldi r24,lo8(9) 854 0196 0E94 0000 call PutChar 301:srx1.c **** PutChar('\t'); 856 .LM59: 857 019a 89E0 ldi r24,lo8(9) 858 019c 0E94 0000 call PutChar 302:srx1.c **** if (BehaviourTable[EXEC].enable) 860 .LM60: 861 01a0 8091 0000 lds r24,BehaviourTable 862 01a4 8823 tst r24 863 01a6 11F0 breq .L17 303:srx1.c **** PutChar('1'); 865 .LM61: 866 01a8 81E3 ldi r24,lo8(49) 867 01aa 01C0 rjmp .L184 868 .L17: 304:srx1.c **** else 305:srx1.c **** PutChar('0'); 870 .LM62: 871 01ac 80E3 ldi r24,lo8(48) 872 .L184: 873 01ae 0E94 0000 call PutChar 306:srx1.c **** PutChar('\t'); 875 .LM63: 876 01b2 89E0 ldi r24,lo8(9) 877 01b4 0E94 0000 call PutChar 307:srx1.c **** if (BehaviourTable[EXEC].active) 879 .LM64: 880 01b8 8091 0000 lds r24,BehaviourTable+1 881 01bc 8823 tst r24 882 01be 11F0 breq .L19 308:srx1.c **** PutChar('1'); 884 .LM65: 885 01c0 81E3 ldi r24,lo8(49) 886 01c2 01C0 rjmp .L185 887 .L19: 309:srx1.c **** else 310:srx1.c **** PutChar('0'); 889 .LM66: 890 01c4 80E3 ldi r24,lo8(48) 891 .L185: 892 01c6 0E94 0000 call PutChar 311:srx1.c **** PutChar('\t'); 894 .LM67: 895 01ca 89E0 ldi r24,lo8(9) 896 01cc 0E94 0000 call PutChar 312:srx1.c **** PutDecByte(BehaviourTable[EXEC].priority); 898 .LM68: 899 01d0 8091 0000 lds r24,BehaviourTable+2 900 01d4 0E94 0000 call PutDecByte 313:srx1.c **** PutChar('\t'); 902 .LM69: 903 01d8 89E0 ldi r24,lo8(9) 904 01da 0E94 0000 call PutChar 314:srx1.c **** PutChar('\t'); 906 .LM70: 907 01de 89E0 ldi r24,lo8(9) 908 01e0 0E94 0000 call PutChar 315:srx1.c **** PutDecSignedWord(BehaviourTable[EXEC].output_l); 910 .LM71: 911 01e4 8091 0000 lds r24,BehaviourTable+3 912 01e8 9091 0000 lds r25,(BehaviourTable+3)+1 913 01ec 0E94 0000 call PutDecSignedWord 316:srx1.c **** PutChar(','); 915 .LM72: 916 01f0 8CE2 ldi r24,lo8(44) 917 01f2 0E94 0000 call PutChar 317:srx1.c **** PutDecSignedWord(BehaviourTable[EXEC].output_r); 919 .LM73: 920 01f6 8091 0000 lds r24,BehaviourTable+5 921 01fa 9091 0000 lds r25,(BehaviourTable+5)+1 922 01fe 0E94 0000 call PutDecSignedWord 318:srx1.c **** PutChar('\t'); 924 .LM74: 925 0202 89E0 ldi r24,lo8(9) 926 0204 0E94 0000 call PutChar 319:srx1.c **** if (BehaviourTable[EXEC].type == ABSOLUTE) 928 .LM75: 929 0208 8091 0000 lds r24,BehaviourTable+8 930 020c 8130 cpi r24,lo8(1) 931 020e 11F4 brne .L21 320:srx1.c **** PutChar('A'); 933 .LM76: 934 0210 81E4 ldi r24,lo8(65) 935 0212 01C0 rjmp .L186 936 .L21: 321:srx1.c **** else 322:srx1.c **** PutChar('R'); 938 .LM77: 939 0214 82E5 ldi r24,lo8(82) 940 .L186: 941 0216 0E94 0000 call PutChar 323:srx1.c **** 324:srx1.c **** PutCRLF(); 943 .LM78: 944 021a 0E94 0000 call PutCRLF 325:srx1.c **** 326:srx1.c **** PrintString(str_follow); // FOLLOW 946 .LM79: 947 021e 80E0 ldi r24,lo8(str_follow) 948 0220 90E0 ldi r25,hi8(str_follow) 949 0222 0E94 0000 call PrintString 327:srx1.c **** PutChar('\t'); 951 .LM80: 952 0226 89E0 ldi r24,lo8(9) 953 0228 0E94 0000 call PutChar 328:srx1.c **** PutChar('\t'); 955 .LM81: 956 022c 89E0 ldi r24,lo8(9) 957 022e 0E94 0000 call PutChar 329:srx1.c **** if (BehaviourTable[FOLLOW].enable) 959 .LM82: 960 0232 8091 0000 lds r24,BehaviourTable+9 961 0236 8823 tst r24 962 0238 11F0 breq .L23 330:srx1.c **** PutChar('1'); 964 .LM83: 965 023a 81E3 ldi r24,lo8(49) 966 023c 01C0 rjmp .L187 967 .L23: 331:srx1.c **** else 332:srx1.c **** PutChar('0'); 969 .LM84: 970 023e 80E3 ldi r24,lo8(48) 971 .L187: 972 0240 0E94 0000 call PutChar 333:srx1.c **** PutChar('\t'); 974 .LM85: 975 0244 89E0 ldi r24,lo8(9) 976 0246 0E94 0000 call PutChar 334:srx1.c **** if (BehaviourTable[FOLLOW].active) 978 .LM86: 979 024a 8091 0000 lds r24,BehaviourTable+10 980 024e 8823 tst r24 981 0250 11F0 breq .L25 335:srx1.c **** PutChar('1'); 983 .LM87: 984 0252 81E3 ldi r24,lo8(49) 985 0254 01C0 rjmp .L188 986 .L25: 336:srx1.c **** else 337:srx1.c **** PutChar('0'); 988 .LM88: 989 0256 80E3 ldi r24,lo8(48) 990 .L188: 991 0258 0E94 0000 call PutChar 338:srx1.c **** PutChar('\t'); 993 .LM89: 994 025c 89E0 ldi r24,lo8(9) 995 025e 0E94 0000 call PutChar 339:srx1.c **** PutDecByte(BehaviourTable[FOLLOW].priority); 997 .LM90: 998 0262 8091 0000 lds r24,BehaviourTable+11 999 0266 0E94 0000 call PutDecByte 340:srx1.c **** PutChar('\t'); 1001 .LM91: 1002 026a 89E0 ldi r24,lo8(9) 1003 026c 0E94 0000 call PutChar 341:srx1.c **** PutChar('\t'); 1005 .LM92: 1006 0270 89E0 ldi r24,lo8(9) 1007 0272 0E94 0000 call PutChar 342:srx1.c **** PutDecSignedWord(BehaviourTable[FOLLOW].output_l); 1009 .LM93: 1010 0276 8091 0000 lds r24,BehaviourTable+12 1011 027a 9091 0000 lds r25,(BehaviourTable+12)+1 1012 027e 0E94 0000 call PutDecSignedWord 343:srx1.c **** PutChar(','); 1014 .LM94: 1015 0282 8CE2 ldi r24,lo8(44) 1016 0284 0E94 0000 call PutChar 344:srx1.c **** PutDecSignedWord(BehaviourTable[FOLLOW].output_r); 1018 .LM95: 1019 0288 8091 0000 lds r24,BehaviourTable+14 1020 028c 9091 0000 lds r25,(BehaviourTable+14)+1 1021 0290 0E94 0000 call PutDecSignedWord 345:srx1.c **** PutChar('\t'); 1023 .LM96: 1024 0294 89E0 ldi r24,lo8(9) 1025 0296 0E94 0000 call PutChar 346:srx1.c **** if (BehaviourTable[FOLLOW].type == ABSOLUTE) 1027 .LM97: 1028 029a 8091 0000 lds r24,BehaviourTable+17 1029 029e 8130 cpi r24,lo8(1) 1030 02a0 11F4 brne .L27 347:srx1.c **** PutChar('A'); 1032 .LM98: 1033 02a2 81E4 ldi r24,lo8(65) 1034 02a4 01C0 rjmp .L189 1035 .L27: 348:srx1.c **** else 349:srx1.c **** PutChar('R'); 1037 .LM99: 1038 02a6 82E5 ldi r24,lo8(82) 1039 .L189: 1040 02a8 0E94 0000 call PutChar 350:srx1.c **** 351:srx1.c **** PutCRLF(); 1042 .LM100: 1043 02ac 0E94 0000 call PutCRLF 352:srx1.c **** 353:srx1.c **** PrintString(str_avoid); // AVOID 1045 .LM101: 1046 02b0 80E0 ldi r24,lo8(str_avoid) 1047 02b2 90E0 ldi r25,hi8(str_avoid) 1048 02b4 0E94 0000 call PrintString 354:srx1.c **** PutChar('\t'); 1050 .LM102: 1051 02b8 89E0 ldi r24,lo8(9) 1052 02ba 0E94 0000 call PutChar 355:srx1.c **** PutChar('\t'); 1054 .LM103: 1055 02be 89E0 ldi r24,lo8(9) 1056 02c0 0E94 0000 call PutChar 356:srx1.c **** if (BehaviourTable[AVOID].enable) 1058 .LM104: 1059 02c4 8091 0000 lds r24,BehaviourTable+45 1060 02c8 8823 tst r24 1061 02ca 11F0 breq .L29 357:srx1.c **** PutChar('1'); 1063 .LM105: 1064 02cc 81E3 ldi r24,lo8(49) 1065 02ce 01C0 rjmp .L190 1066 .L29: 358:srx1.c **** else 359:srx1.c **** PutChar('0'); 1068 .LM106: 1069 02d0 80E3 ldi r24,lo8(48) 1070 .L190: 1071 02d2 0E94 0000 call PutChar 360:srx1.c **** PutChar('\t'); 1073 .LM107: 1074 02d6 89E0 ldi r24,lo8(9) 1075 02d8 0E94 0000 call PutChar 361:srx1.c **** if (BehaviourTable[AVOID].active) 1077 .LM108: 1078 02dc 8091 0000 lds r24,BehaviourTable+46 1079 02e0 8823 tst r24 1080 02e2 11F0 breq .L31 362:srx1.c **** PutChar('1'); 1082 .LM109: 1083 02e4 81E3 ldi r24,lo8(49) 1084 02e6 01C0 rjmp .L191 1085 .L31: 363:srx1.c **** else 364:srx1.c **** PutChar('0'); 1087 .LM110: 1088 02e8 80E3 ldi r24,lo8(48) 1089 .L191: 1090 02ea 0E94 0000 call PutChar 365:srx1.c **** PutChar('\t'); 1092 .LM111: 1093 02ee 89E0 ldi r24,lo8(9) 1094 02f0 0E94 0000 call PutChar 366:srx1.c **** PutDecByte(BehaviourTable[AVOID].priority); 1096 .LM112: 1097 02f4 8091 0000 lds r24,BehaviourTable+47 1098 02f8 0E94 0000 call PutDecByte 367:srx1.c **** PutChar('\t'); 1100 .LM113: 1101 02fc 89E0 ldi r24,lo8(9) 1102 02fe 0E94 0000 call PutChar 368:srx1.c **** PutChar('\t'); 1104 .LM114: 1105 0302 89E0 ldi r24,lo8(9) 1106 0304 0E94 0000 call PutChar 369:srx1.c **** PutDecSignedWord(BehaviourTable[AVOID].output_l); 1108 .LM115: 1109 0308 8091 0000 lds r24,BehaviourTable+48 1110 030c 9091 0000 lds r25,(BehaviourTable+48)+1 1111 0310 0E94 0000 call PutDecSignedWord 370:srx1.c **** PutChar(','); 1113 .LM116: 1114 0314 8CE2 ldi r24,lo8(44) 1115 0316 0E94 0000 call PutChar 371:srx1.c **** PutDecSignedWord(BehaviourTable[AVOID].output_r); 1117 .LM117: 1118 031a 8091 0000 lds r24,BehaviourTable+50 1119 031e 9091 0000 lds r25,(BehaviourTable+50)+1 1120 0322 0E94 0000 call PutDecSignedWord 372:srx1.c **** PutChar('\t'); 1122 .LM118: 1123 0326 89E0 ldi r24,lo8(9) 1124 0328 0E94 0000 call PutChar 373:srx1.c **** if (BehaviourTable[AVOID].type == ABSOLUTE) 1126 .LM119: 1127 032c 8091 0000 lds r24,BehaviourTable+53 1128 0330 8130 cpi r24,lo8(1) 1129 0332 11F4 brne .L33 374:srx1.c **** PutChar('A'); 1131 .LM120: 1132 0334 81E4 ldi r24,lo8(65) 1133 0336 01C0 rjmp .L192 1134 .L33: 375:srx1.c **** else 376:srx1.c **** PutChar('R'); 1136 .LM121: 1137 0338 82E5 ldi r24,lo8(82) 1138 .L192: 1139 033a 0E94 0000 call PutChar 377:srx1.c **** 378:srx1.c **** PutCRLF(); 1141 .LM122: 1142 033e 0E94 0000 call PutCRLF 379:srx1.c **** 380:srx1.c **** PrintString(str_goat); // GOAT 1144 .LM123: 1145 0342 80E0 ldi r24,lo8(str_goat) 1146 0344 90E0 ldi r25,hi8(str_goat) 1147 0346 0E94 0000 call PrintString 381:srx1.c **** PutChar('\t'); 1149 .LM124: 1150 034a 89E0 ldi r24,lo8(9) 1151 034c 0E94 0000 call PutChar 382:srx1.c **** PutChar('\t'); 1153 .LM125: 1154 0350 89E0 ldi r24,lo8(9) 1155 0352 0E94 0000 call PutChar 383:srx1.c **** if (BehaviourTable[GOAT].enable) 1157 .LM126: 1158 0356 8091 0000 lds r24,BehaviourTable+36 1159 035a 8823 tst r24 1160 035c 11F0 breq .L35 384:srx1.c **** PutChar('1'); 1162 .LM127: 1163 035e 81E3 ldi r24,lo8(49) 1164 0360 01C0 rjmp .L193 1165 .L35: 385:srx1.c **** else 386:srx1.c **** PutChar('0'); 1167 .LM128: 1168 0362 80E3 ldi r24,lo8(48) 1169 .L193: 1170 0364 0E94 0000 call PutChar 387:srx1.c **** PutChar('\t'); 1172 .LM129: 1173 0368 89E0 ldi r24,lo8(9) 1174 036a 0E94 0000 call PutChar 388:srx1.c **** if (BehaviourTable[GOAT].active) 1176 .LM130: 1177 036e 8091 0000 lds r24,BehaviourTable+37 1178 0372 8823 tst r24 1179 0374 11F0 breq .L37 389:srx1.c **** PutChar('1'); 1181 .LM131: 1182 0376 81E3 ldi r24,lo8(49) 1183 0378 01C0 rjmp .L194 1184 .L37: 390:srx1.c **** else 391:srx1.c **** PutChar('0'); 1186 .LM132: 1187 037a 80E3 ldi r24,lo8(48) 1188 .L194: 1189 037c 0E94 0000 call PutChar 392:srx1.c **** PutChar('\t'); 1191 .LM133: 1192 0380 89E0 ldi r24,lo8(9) 1193 0382 0E94 0000 call PutChar 393:srx1.c **** PutDecByte(BehaviourTable[GOAT].priority); 1195 .LM134: 1196 0386 8091 0000 lds r24,BehaviourTable+38 1197 038a 0E94 0000 call PutDecByte 394:srx1.c **** PutChar('\t'); 1199 .LM135: 1200 038e 89E0 ldi r24,lo8(9) 1201 0390 0E94 0000 call PutChar 395:srx1.c **** PutChar('\t'); 1203 .LM136: 1204 0394 89E0 ldi r24,lo8(9) 1205 0396 0E94 0000 call PutChar 396:srx1.c **** PutDecSignedWord(BehaviourTable[GOAT].output_l); 1207 .LM137: 1208 039a 8091 0000 lds r24,BehaviourTable+39 1209 039e 9091 0000 lds r25,(BehaviourTable+39)+1 1210 03a2 0E94 0000 call PutDecSignedWord 397:srx1.c **** PutChar(','); 1212 .LM138: 1213 03a6 8CE2 ldi r24,lo8(44) 1214 03a8 0E94 0000 call PutChar 398:srx1.c **** PutDecSignedWord(BehaviourTable[GOAT].output_r); 1216 .LM139: 1217 03ac 8091 0000 lds r24,BehaviourTable+41 1218 03b0 9091 0000 lds r25,(BehaviourTable+41)+1 1219 03b4 0E94 0000 call PutDecSignedWord 399:srx1.c **** PutChar('\t'); 1221 .LM140: 1222 03b8 89E0 ldi r24,lo8(9) 1223 03ba 0E94 0000 call PutChar 400:srx1.c **** if (BehaviourTable[GOAT].type == ABSOLUTE) 1225 .LM141: 1226 03be 8091 0000 lds r24,BehaviourTable+44 1227 03c2 8130 cpi r24,lo8(1) 1228 03c4 11F4 brne .L39 401:srx1.c **** PutChar('A'); 1230 .LM142: 1231 03c6 81E4 ldi r24,lo8(65) 1232 03c8 01C0 rjmp .L195 1233 .L39: 402:srx1.c **** else 403:srx1.c **** PutChar('R'); 1235 .LM143: 1236 03ca 82E5 ldi r24,lo8(82) 1237 .L195: 1238 03cc 0E94 0000 call PutChar 404:srx1.c **** 405:srx1.c **** PutCRLF(); 1240 .LM144: 1241 03d0 0E94 0000 call PutCRLF 406:srx1.c **** 407:srx1.c **** PrintString(str_extinguish); // EXTINGUISH 1243 .LM145: 1244 03d4 80E0 ldi r24,lo8(str_extinguish) 1245 03d6 90E0 ldi r25,hi8(str_extinguish) 1246 03d8 0E94 0000 call PrintString 408:srx1.c **** PutChar('\t'); 1248 .LM146: 1249 03dc 89E0 ldi r24,lo8(9) 1250 03de 0E94 0000 call PutChar 409:srx1.c **** if (BehaviourTable[EXTINGUISH].enable) 1252 .LM147: 1253 03e2 8091 0000 lds r24,BehaviourTable+63 1254 03e6 8823 tst r24 1255 03e8 11F0 breq .L41 410:srx1.c **** PutChar('1'); 1257 .LM148: 1258 03ea 81E3 ldi r24,lo8(49) 1259 03ec 01C0 rjmp .L196 1260 .L41: 411:srx1.c **** else 412:srx1.c **** PutChar('0'); 1262 .LM149: 1263 03ee 80E3 ldi r24,lo8(48) 1264 .L196: 1265 03f0 0E94 0000 call PutChar 413:srx1.c **** PutChar('\t'); 1267 .LM150: 1268 03f4 89E0 ldi r24,lo8(9) 1269 03f6 0E94 0000 call PutChar 414:srx1.c **** if (BehaviourTable[EXTINGUISH].active) 1271 .LM151: 1272 03fa 8091 0000 lds r24,BehaviourTable+64 1273 03fe 8823 tst r24 1274 0400 11F0 breq .L43 415:srx1.c **** PutChar('1'); 1276 .LM152: 1277 0402 81E3 ldi r24,lo8(49) 1278 0404 01C0 rjmp .L197 1279 .L43: 416:srx1.c **** else 417:srx1.c **** PutChar('0'); 1281 .LM153: 1282 0406 80E3 ldi r24,lo8(48) 1283 .L197: 1284 0408 0E94 0000 call PutChar 418:srx1.c **** PutChar('\t'); 1286 .LM154: 1287 040c 89E0 ldi r24,lo8(9) 1288 040e 0E94 0000 call PutChar 419:srx1.c **** PutDecByte(BehaviourTable[EXTINGUISH].priority); 1290 .LM155: 1291 0412 8091 0000 lds r24,BehaviourTable+65 1292 0416 0E94 0000 call PutDecByte 420:srx1.c **** PutChar('\t'); 1294 .LM156: 1295 041a 89E0 ldi r24,lo8(9) 1296 041c 0E94 0000 call PutChar 421:srx1.c **** PutChar('\t'); 1298 .LM157: 1299 0420 89E0 ldi r24,lo8(9) 1300 0422 0E94 0000 call PutChar 422:srx1.c **** PutDecSignedWord(BehaviourTable[EXTINGUISH].output_l); 1302 .LM158: 1303 0426 8091 0000 lds r24,BehaviourTable+66 1304 042a 9091 0000 lds r25,(BehaviourTable+66)+1 1305 042e 0E94 0000 call PutDecSignedWord 423:srx1.c **** PutChar(','); 1307 .LM159: 1308 0432 8CE2 ldi r24,lo8(44) 1309 0434 0E94 0000 call PutChar 424:srx1.c **** PutDecSignedWord(BehaviourTable[EXTINGUISH].output_r); 1311 .LM160: 1312 0438 8091 0000 lds r24,BehaviourTable+68 1313 043c 9091 0000 lds r25,(BehaviourTable+68)+1 1314 0440 0E94 0000 call PutDecSignedWord 425:srx1.c **** PutChar('\t'); 1316 .LM161: 1317 0444 89E0 ldi r24,lo8(9) 1318 0446 0E94 0000 call PutChar 426:srx1.c **** if (BehaviourTable[EXTINGUISH].type == ABSOLUTE) 1320 .LM162: 1321 044a 8091 0000 lds r24,BehaviourTable+71 1322 044e 8130 cpi r24,lo8(1) 1323 0450 11F4 brne .L45 427:srx1.c **** PutChar('A'); 1325 .LM163: 1326 0452 81E4 ldi r24,lo8(65) 1327 0454 01C0 rjmp .L198 1328 .L45: 428:srx1.c **** else 429:srx1.c **** PutChar('R'); 1330 .LM164: 1331 0456 82E5 ldi r24,lo8(82) 1332 .L198: 1333 0458 0E94 0000 call PutChar 430:srx1.c **** PutCRLF(); 1335 .LM165: 1336 045c 0E94 0000 call PutCRLF 431:srx1.c **** 432:srx1.c **** PrintString(str_rotate); // ROTATE 1338 .LM166: 1339 0460 80E0 ldi r24,lo8(str_rotate) 1340 0462 90E0 ldi r25,hi8(str_rotate) 1341 0464 0E94 0000 call PrintString 433:srx1.c **** PutChar('\t'); 1343 .LM167: 1344 0468 89E0 ldi r24,lo8(9) 1345 046a 0E94 0000 call PutChar 434:srx1.c **** PutChar('\t'); 1347 .LM168: 1348 046e 89E0 ldi r24,lo8(9) 1349 0470 0E94 0000 call PutChar 435:srx1.c **** if (BehaviourTable[ROTATE].enable) 1351 .LM169: 1352 0474 8091 0000 lds r24,BehaviourTable+18 1353 0478 8823 tst r24 1354 047a 11F0 breq .L47 436:srx1.c **** PutChar('1'); 1356 .LM170: 1357 047c 81E3 ldi r24,lo8(49) 1358 047e 01C0 rjmp .L199 1359 .L47: 437:srx1.c **** else 438:srx1.c **** PutChar('0'); 1361 .LM171: 1362 0480 80E3 ldi r24,lo8(48) 1363 .L199: 1364 0482 0E94 0000 call PutChar 439:srx1.c **** PutChar('\t'); 1366 .LM172: 1367 0486 89E0 ldi r24,lo8(9) 1368 0488 0E94 0000 call PutChar 440:srx1.c **** if (BehaviourTable[ROTATE].active) 1370 .LM173: 1371 048c 8091 0000 lds r24,BehaviourTable+19 1372 0490 8823 tst r24 1373 0492 11F0 breq .L49 441:srx1.c **** PutChar('1'); 1375 .LM174: 1376 0494 81E3 ldi r24,lo8(49) 1377 0496 01C0 rjmp .L200 1378 .L49: 442:srx1.c **** else 443:srx1.c **** PutChar('0'); 1380 .LM175: 1381 0498 80E3 ldi r24,lo8(48) 1382 .L200: 1383 049a 0E94 0000 call PutChar 444:srx1.c **** PutChar('\t'); 1385 .LM176: 1386 049e 89E0 ldi r24,lo8(9) 1387 04a0 0E94 0000 call PutChar 445:srx1.c **** PutDecByte(BehaviourTable[ROTATE].priority); 1389 .LM177: 1390 04a4 8091 0000 lds r24,BehaviourTable+20 1391 04a8 0E94 0000 call PutDecByte 446:srx1.c **** PutChar('\t'); 1393 .LM178: 1394 04ac 89E0 ldi r24,lo8(9) 1395 04ae 0E94 0000 call PutChar 447:srx1.c **** PutChar('\t'); 1397 .LM179: 1398 04b2 89E0 ldi r24,lo8(9) 1399 04b4 0E94 0000 call PutChar 448:srx1.c **** PutDecSignedWord(BehaviourTable[ROTATE].output_l); 1401 .LM180: 1402 04b8 8091 0000 lds r24,BehaviourTable+21 1403 04bc 9091 0000 lds r25,(BehaviourTable+21)+1 1404 04c0 0E94 0000 call PutDecSignedWord 449:srx1.c **** PutChar(','); 1406 .LM181: 1407 04c4 8CE2 ldi r24,lo8(44) 1408 04c6 0E94 0000 call PutChar 450:srx1.c **** PutDecSignedWord(BehaviourTable[ROTATE].output_r); 1410 .LM182: 1411 04ca 8091 0000 lds r24,BehaviourTable+23 1412 04ce 9091 0000 lds r25,(BehaviourTable+23)+1 1413 04d2 0E94 0000 call PutDecSignedWord 451:srx1.c **** PutChar('\t'); 1415 .LM183: 1416 04d6 89E0 ldi r24,lo8(9) 1417 04d8 0E94 0000 call PutChar 452:srx1.c **** if (BehaviourTable[ROTATE].type == ABSOLUTE) 1419 .LM184: 1420 04dc 8091 0000 lds r24,BehaviourTable+26 1421 04e0 8130 cpi r24,lo8(1) 1422 04e2 11F4 brne .L51 453:srx1.c **** PutChar('A'); 1424 .LM185: 1425 04e4 81E4 ldi r24,lo8(65) 1426 04e6 01C0 rjmp .L201 1427 .L51: 454:srx1.c **** else 455:srx1.c **** PutChar('R'); 1429 .LM186: 1430 04e8 82E5 ldi r24,lo8(82) 1431 .L201: 1432 04ea 0E94 0000 call PutChar 456:srx1.c **** 457:srx1.c **** PutCRLF(); 1434 .LM187: 1435 04ee 0E94 0000 call PutCRLF 458:srx1.c **** 459:srx1.c **** PrintString(str_align); // ALIGN 1437 .LM188: 1438 04f2 80E0 ldi r24,lo8(str_align) 1439 04f4 90E0 ldi r25,hi8(str_align) 1440 04f6 0E94 0000 call PrintString 460:srx1.c **** PutChar('\t'); 1442 .LM189: 1443 04fa 89E0 ldi r24,lo8(9) 1444 04fc 0E94 0000 call PutChar 461:srx1.c **** PutChar('\t'); 1446 .LM190: 1447 0500 89E0 ldi r24,lo8(9) 1448 0502 0E94 0000 call PutChar 462:srx1.c **** if (BehaviourTable[ALIGN].enable) 1450 .LM191: 1451 0506 8091 0000 lds r24,BehaviourTable+81 1452 050a 8823 tst r24 1453 050c 11F0 breq .L53 463:srx1.c **** PutChar('1'); 1455 .LM192: 1456 050e 81E3 ldi r24,lo8(49) 1457 0510 01C0 rjmp .L202 1458 .L53: 464:srx1.c **** else 465:srx1.c **** PutChar('0'); 1460 .LM193: 1461 0512 80E3 ldi r24,lo8(48) 1462 .L202: 1463 0514 0E94 0000 call PutChar 466:srx1.c **** PutChar('\t'); 1465 .LM194: 1466 0518 89E0 ldi r24,lo8(9) 1467 051a 0E94 0000 call PutChar 467:srx1.c **** if (BehaviourTable[ALIGN].active) 1469 .LM195: 1470 051e 8091 0000 lds r24,BehaviourTable+82 1471 0522 8823 tst r24 1472 0524 11F0 breq .L55 468:srx1.c **** PutChar('1'); 1474 .LM196: 1475 0526 81E3 ldi r24,lo8(49) 1476 0528 01C0 rjmp .L203 1477 .L55: 469:srx1.c **** else 470:srx1.c **** PutChar('0'); 1479 .LM197: 1480 052a 80E3 ldi r24,lo8(48) 1481 .L203: 1482 052c 0E94 0000 call PutChar 471:srx1.c **** PutChar('\t'); 1484 .LM198: 1485 0530 89E0 ldi r24,lo8(9) 1486 0532 0E94 0000 call PutChar 472:srx1.c **** PutDecByte(BehaviourTable[ALIGN].priority); 1488 .LM199: 1489 0536 8091 0000 lds r24,BehaviourTable+83 1490 053a 0E94 0000 call PutDecByte 473:srx1.c **** PutChar('\t'); 1492 .LM200: 1493 053e 89E0 ldi r24,lo8(9) 1494 0540 0E94 0000 call PutChar 474:srx1.c **** PutChar('\t'); 1496 .LM201: 1497 0544 89E0 ldi r24,lo8(9) 1498 0546 0E94 0000 call PutChar 475:srx1.c **** PutDecSignedWord(BehaviourTable[ALIGN].output_l); 1500 .LM202: 1501 054a 8091 0000 lds r24,BehaviourTable+84 1502 054e 9091 0000 lds r25,(BehaviourTable+84)+1 1503 0552 0E94 0000 call PutDecSignedWord 476:srx1.c **** PutChar(','); 1505 .LM203: 1506 0556 8CE2 ldi r24,lo8(44) 1507 0558 0E94 0000 call PutChar 477:srx1.c **** PutDecSignedWord(BehaviourTable[ALIGN].output_r); 1509 .LM204: 1510 055c 8091 0000 lds r24,BehaviourTable+86 1511 0560 9091 0000 lds r25,(BehaviourTable+86)+1 1512 0564 0E94 0000 call PutDecSignedWord 478:srx1.c **** PutChar('\t'); 1514 .LM205: 1515 0568 89E0 ldi r24,lo8(9) 1516 056a 0E94 0000 call PutChar 479:srx1.c **** if (BehaviourTable[ALIGN].type == ABSOLUTE) 1518 .LM206: 1519 056e 8091 0000 lds r24,BehaviourTable+89 1520 0572 8130 cpi r24,lo8(1) 1521 0574 11F4 brne .L57 480:srx1.c **** PutChar('A'); 1523 .LM207: 1524 0576 81E4 ldi r24,lo8(65) 1525 0578 01C0 rjmp .L204 1526 .L57: 481:srx1.c **** else 482:srx1.c **** PutChar('R'); 1528 .LM208: 1529 057a 82E5 ldi r24,lo8(82) 1530 .L204: 1531 057c 0E94 0000 call PutChar 483:srx1.c **** 484:srx1.c **** PutCRLF(); 1533 .LM209: 1534 0580 0E94 0000 call PutCRLF 485:srx1.c **** 486:srx1.c **** PrintString(str_forward); // FORWARD 1536 .LM210: 1537 0584 80E0 ldi r24,lo8(str_forward) 1538 0586 90E0 ldi r25,hi8(str_forward) 1539 0588 0E94 0000 call PrintString 487:srx1.c **** PutChar('\t'); 1541 .LM211: 1542 058c 89E0 ldi r24,lo8(9) 1543 058e 0E94 0000 call PutChar 488:srx1.c **** PutChar('\t'); 1545 .LM212: 1546 0592 89E0 ldi r24,lo8(9) 1547 0594 0E94 0000 call PutChar 489:srx1.c **** if (BehaviourTable[FORWARD].enable) 1549 .LM213: 1550 0598 8091 0000 lds r24,BehaviourTable+27 1551 059c 8823 tst r24 1552 059e 11F0 breq .L59 490:srx1.c **** PutChar('1'); 1554 .LM214: 1555 05a0 81E3 ldi r24,lo8(49) 1556 05a2 01C0 rjmp .L205 1557 .L59: 491:srx1.c **** else 492:srx1.c **** PutChar('0'); 1559 .LM215: 1560 05a4 80E3 ldi r24,lo8(48) 1561 .L205: 1562 05a6 0E94 0000 call PutChar 493:srx1.c **** PutChar('\t'); 1564 .LM216: 1565 05aa 89E0 ldi r24,lo8(9) 1566 05ac 0E94 0000 call PutChar 494:srx1.c **** if (BehaviourTable[FORWARD].active) 1568 .LM217: 1569 05b0 8091 0000 lds r24,BehaviourTable+28 1570 05b4 8823 tst r24 1571 05b6 11F0 breq .L61 495:srx1.c **** PutChar('1'); 1573 .LM218: 1574 05b8 81E3 ldi r24,lo8(49) 1575 05ba 01C0 rjmp .L206 1576 .L61: 496:srx1.c **** else 497:srx1.c **** PutChar('0'); 1578 .LM219: 1579 05bc 80E3 ldi r24,lo8(48) 1580 .L206: 1581 05be 0E94 0000 call PutChar 498:srx1.c **** PutChar('\t'); 1583 .LM220: 1584 05c2 89E0 ldi r24,lo8(9) 1585 05c4 0E94 0000 call PutChar 499:srx1.c **** PutDecByte(BehaviourTable[FORWARD].priority); 1587 .LM221: 1588 05c8 8091 0000 lds r24,BehaviourTable+29 1589 05cc 0E94 0000 call PutDecByte 500:srx1.c **** PutChar('\t'); 1591 .LM222: 1592 05d0 89E0 ldi r24,lo8(9) 1593 05d2 0E94 0000 call PutChar 501:srx1.c **** PutChar('\t'); 1595 .LM223: 1596 05d6 89E0 ldi r24,lo8(9) 1597 05d8 0E94 0000 call PutChar 502:srx1.c **** PutDecSignedWord(BehaviourTable[FORWARD].output_l); 1599 .LM224: 1600 05dc 8091 0000 lds r24,BehaviourTable+30 1601 05e0 9091 0000 lds r25,(BehaviourTable+30)+1 1602 05e4 0E94 0000 call PutDecSignedWord 503:srx1.c **** PutChar(','); 1604 .LM225: 1605 05e8 8CE2 ldi r24,lo8(44) 1606 05ea 0E94 0000 call PutChar 504:srx1.c **** PutDecSignedWord(BehaviourTable[FORWARD].output_r); 1608 .LM226: 1609 05ee 8091 0000 lds r24,BehaviourTable+32 1610 05f2 9091 0000 lds r25,(BehaviourTable+32)+1 1611 05f6 0E94 0000 call PutDecSignedWord 505:srx1.c **** PutChar('\t'); 1613 .LM227: 1614 05fa 89E0 ldi r24,lo8(9) 1615 05fc 0E94 0000 call PutChar 506:srx1.c **** if (BehaviourTable[FORWARD].type == ABSOLUTE) 1617 .LM228: 1618 0600 8091 0000 lds r24,BehaviourTable+35 1619 0604 8130 cpi r24,lo8(1) 1620 0606 11F4 brne .L63 507:srx1.c **** PutChar('A'); 1622 .LM229: 1623 0608 81E4 ldi r24,lo8(65) 1624 060a 01C0 rjmp .L207 1625 .L63: 508:srx1.c **** else 509:srx1.c **** PutChar('R'); 1627 .LM230: 1628 060c 82E5 ldi r24,lo8(82) 1629 .L207: 1630 060e 0E94 0000 call PutChar 510:srx1.c **** 511:srx1.c **** PutCRLF(); 1632 .LM231: 1633 0612 0E94 0000 call PutCRLF 512:srx1.c **** 513:srx1.c **** PrintString(str_search); // SEARCH 1635 .LM232: 1636 0616 80E0 ldi r24,lo8(str_search) 1637 0618 90E0 ldi r25,hi8(str_search) 1638 061a 0E94 0000 call PrintString 514:srx1.c **** PutChar('\t'); 1640 .LM233: 1641 061e 89E0 ldi r24,lo8(9) 1642 0620 0E94 0000 call PutChar 515:srx1.c **** PutChar('\t'); 1644 .LM234: 1645 0624 89E0 ldi r24,lo8(9) 1646 0626 0E94 0000 call PutChar 516:srx1.c **** if (BehaviourTable[SEARCH].enable) 1648 .LM235: 1649 062a 8091 0000 lds r24,BehaviourTable+72 1650 062e 8823 tst r24 1651 0630 11F0 breq .L65 517:srx1.c **** PutChar('1'); 1653 .LM236: 1654 0632 81E3 ldi r24,lo8(49) 1655 0634 01C0 rjmp .L208 1656 .L65: 518:srx1.c **** else 519:srx1.c **** PutChar('0'); 1658 .LM237: 1659 0636 80E3 ldi r24,lo8(48) 1660 .L208: 1661 0638 0E94 0000 call PutChar 520:srx1.c **** PutChar('\t'); 1663 .LM238: 1664 063c 89E0 ldi r24,lo8(9) 1665 063e 0E94 0000 call PutChar 521:srx1.c **** if (BehaviourTable[SEARCH].active) 1667 .LM239: 1668 0642 8091 0000 lds r24,BehaviourTable+73 1669 0646 8823 tst r24 1670 0648 11F0 breq .L67 522:srx1.c **** PutChar('1'); 1672 .LM240: 1673 064a 81E3 ldi r24,lo8(49) 1674 064c 01C0 rjmp .L209 1675 .L67: 523:srx1.c **** else 524:srx1.c **** PutChar('0'); 1677 .LM241: 1678 064e 80E3 ldi r24,lo8(48) 1679 .L209: 1680 0650 0E94 0000 call PutChar 525:srx1.c **** PutChar('\t'); 1682 .LM242: 1683 0654 89E0 ldi r24,lo8(9) 1684 0656 0E94 0000 call PutChar 526:srx1.c **** PutDecByte(BehaviourTable[SEARCH].priority); 1686 .LM243: 1687 065a 8091 0000 lds r24,BehaviourTable+74 1688 065e 0E94 0000 call PutDecByte 527:srx1.c **** PutChar('\t'); 1690 .LM244: 1691 0662 89E0 ldi r24,lo8(9) 1692 0664 0E94 0000 call PutChar 528:srx1.c **** PutChar('\t'); 1694 .LM245: 1695 0668 89E0 ldi r24,lo8(9) 1696 066a 0E94 0000 call PutChar 529:srx1.c **** PutDecSignedWord(BehaviourTable[SEARCH].output_l); 1698 .LM246: 1699 066e 8091 0000 lds r24,BehaviourTable+75 1700 0672 9091 0000 lds r25,(BehaviourTable+75)+1 1701 0676 0E94 0000 call PutDecSignedWord 530:srx1.c **** PutChar(','); 1703 .LM247: 1704 067a 8CE2 ldi r24,lo8(44) 1705 067c 0E94 0000 call PutChar 531:srx1.c **** PutDecSignedWord(BehaviourTable[SEARCH].output_r); 1707 .LM248: 1708 0680 8091 0000 lds r24,BehaviourTable+77 1709 0684 9091 0000 lds r25,(BehaviourTable+77)+1 1710 0688 0E94 0000 call PutDecSignedWord 532:srx1.c **** PutChar('\t'); 1712 .LM249: 1713 068c 89E0 ldi r24,lo8(9) 1714 068e 0E94 0000 call PutChar 533:srx1.c **** if (BehaviourTable[SEARCH].type == ABSOLUTE) 1716 .LM250: 1717 0692 8091 0000 lds r24,BehaviourTable+80 1718 0696 8130 cpi r24,lo8(1) 1719 0698 11F4 brne .L69 534:srx1.c **** PutChar('A'); 1721 .LM251: 1722 069a 81E4 ldi r24,lo8(65) 1723 069c 01C0 rjmp .L210 1724 .L69: 535:srx1.c **** else 536:srx1.c **** PutChar('R'); 1726 .LM252: 1727 069e 82E5 ldi r24,lo8(82) 1728 .L210: 1729 06a0 0E94 0000 call PutChar 537:srx1.c **** 538:srx1.c **** PutCRLF(); 1731 .LM253: 1732 06a4 0E94 0000 call PutCRLF 539:srx1.c **** 540:srx1.c **** PrintString(str_roam); // ROAM 1734 .LM254: 1735 06a8 80E0 ldi r24,lo8(str_roam) 1736 06aa 90E0 ldi r25,hi8(str_roam) 1737 06ac 0E94 0000 call PrintString 541:srx1.c **** PutChar('\t'); 1739 .LM255: 1740 06b0 89E0 ldi r24,lo8(9) 1741 06b2 0E94 0000 call PutChar 542:srx1.c **** PutChar('\t'); 1743 .LM256: 1744 06b6 89E0 ldi r24,lo8(9) 1745 06b8 0E94 0000 call PutChar 543:srx1.c **** if (BehaviourTable[ROAM].enable) 1747 .LM257: 1748 06bc 8091 0000 lds r24,BehaviourTable+54 1749 06c0 8823 tst r24 1750 06c2 11F0 breq .L71 544:srx1.c **** PutChar('1'); 1752 .LM258: 1753 06c4 81E3 ldi r24,lo8(49) 1754 06c6 01C0 rjmp .L211 1755 .L71: 545:srx1.c **** else 546:srx1.c **** PutChar('0'); 1757 .LM259: 1758 06c8 80E3 ldi r24,lo8(48) 1759 .L211: 1760 06ca 0E94 0000 call PutChar 547:srx1.c **** PutChar('\t'); 1762 .LM260: 1763 06ce 89E0 ldi r24,lo8(9) 1764 06d0 0E94 0000 call PutChar 548:srx1.c **** if (BehaviourTable[ROAM].active) 1766 .LM261: 1767 06d4 8091 0000 lds r24,BehaviourTable+55 1768 06d8 8823 tst r24 1769 06da 11F0 breq .L73 549:srx1.c **** PutChar('1'); 1771 .LM262: 1772 06dc 81E3 ldi r24,lo8(49) 1773 06de 01C0 rjmp .L212 1774 .L73: 550:srx1.c **** else 551:srx1.c **** PutChar('0'); 1776 .LM263: 1777 06e0 80E3 ldi r24,lo8(48) 1778 .L212: 1779 06e2 0E94 0000 call PutChar 552:srx1.c **** PutChar('\t'); 1781 .LM264: 1782 06e6 89E0 ldi r24,lo8(9) 1783 06e8 0E94 0000 call PutChar 553:srx1.c **** PutDecByte(BehaviourTable[ROAM].priority); 1785 .LM265: 1786 06ec 8091 0000 lds r24,BehaviourTable+56 1787 06f0 0E94 0000 call PutDecByte 554:srx1.c **** PutChar('\t'); 1789 .LM266: 1790 06f4 89E0 ldi r24,lo8(9) 1791 06f6 0E94 0000 call PutChar 555:srx1.c **** PutChar('\t'); 1793 .LM267: 1794 06fa 89E0 ldi r24,lo8(9) 1795 06fc 0E94 0000 call PutChar 556:srx1.c **** PutDecSignedWord(BehaviourTable[ROAM].output_l); 1797 .LM268: 1798 0700 8091 0000 lds r24,BehaviourTable+57 1799 0704 9091 0000 lds r25,(BehaviourTable+57)+1 1800 0708 0E94 0000 call PutDecSignedWord 557:srx1.c **** PutChar(','); 1802 .LM269: 1803 070c 8CE2 ldi r24,lo8(44) 1804 070e 0E94 0000 call PutChar 558:srx1.c **** PutDecSignedWord(BehaviourTable[ROAM].output_r); 1806 .LM270: 1807 0712 8091 0000 lds r24,BehaviourTable+59 1808 0716 9091 0000 lds r25,(BehaviourTable+59)+1 1809 071a 0E94 0000 call PutDecSignedWord 559:srx1.c **** PutChar('\t'); 1811 .LM271: 1812 071e 89E0 ldi r24,lo8(9) 1813 0720 0E94 0000 call PutChar 560:srx1.c **** if (BehaviourTable[ROAM].type == ABSOLUTE) 1815 .LM272: 1816 0724 8091 0000 lds r24,BehaviourTable+62 1817 0728 8130 cpi r24,lo8(1) 1818 072a 11F4 brne .L75 561:srx1.c **** PutChar('A'); 1820 .LM273: 1821 072c 81E4 ldi r24,lo8(65) 1822 072e 01C0 rjmp .L213 1823 .L75: 562:srx1.c **** else 563:srx1.c **** PutChar('R'); 1825 .LM274: 1826 0730 82E5 ldi r24,lo8(82) 1827 .L213: 1828 0732 0E94 0000 call PutChar 564:srx1.c **** 565:srx1.c **** PrintString(str_spacer); 1830 .LM275: 1831 0736 80E0 ldi r24,lo8(str_spacer) 1832 0738 90E0 ldi r25,hi8(str_spacer) 1833 073a 0E94 0000 call PrintString 566:srx1.c **** 567:srx1.c **** // TEMP: Display PID coefficients 568:srx1.c **** PutCRLF(); 1835 .LM276: 1836 073e 0E94 0000 call PutCRLF 569:srx1.c **** PutChar('P'); 1838 .LM277: 1839 0742 80E5 ldi r24,lo8(80) 1840 0744 0E94 0000 call PutChar 570:srx1.c **** PutChar('='); 1842 .LM278: 1843 0748 8DE3 ldi r24,lo8(61) 1844 074a 0E94 0000 call PutChar 571:srx1.c **** PutDecSignedByte(R_ParameterTable.Kp/K_O); 1846 .LM279: 1847 074e 8091 0000 lds r24,R_ParameterTable+3 1848 0752 6AE0 ldi r22,lo8(10) 1849 0754 0E94 0000 call __udivmodqi4 1850 0758 0E94 0000 call PutDecSignedByte 572:srx1.c **** PutChar('.'); 1852 .LM280: 1853 075c 8EE2 ldi r24,lo8(46) 1854 075e 0E94 0000 call PutChar 573:srx1.c **** PutDecByte(R_ParameterTable.Kp%K_O); 1856 .LM281: 1857 0762 8091 0000 lds r24,R_ParameterTable+3 1858 0766 6AE0 ldi r22,lo8(10) 1859 0768 0E94 0000 call __udivmodqi4 1860 076c 892F mov r24,r25 1861 076e 0E94 0000 call PutDecByte 574:srx1.c **** PutChar(' '); 1863 .LM282: 1864 0772 80E2 ldi r24,lo8(32) 1865 0774 0E94 0000 call PutChar 575:srx1.c **** PutChar('I'); 1867 .LM283: 1868 0778 89E4 ldi r24,lo8(73) 1869 077a 0E94 0000 call PutChar 576:srx1.c **** PutChar('='); 1871 .LM284: 1872 077e 8DE3 ldi r24,lo8(61) 1873 0780 0E94 0000 call PutChar 577:srx1.c **** PutDecSignedByte(R_ParameterTable.Ki/K_O); 1875 .LM285: 1876 0784 8091 0000 lds r24,R_ParameterTable+4 1877 0788 6AE0 ldi r22,lo8(10) 1878 078a 0E94 0000 call __udivmodqi4 1879 078e 0E94 0000 call PutDecSignedByte 578:srx1.c **** PutChar('.'); 1881 .LM286: 1882 0792 8EE2 ldi r24,lo8(46) 1883 0794 0E94 0000 call PutChar 579:srx1.c **** PutDecByte(R_ParameterTable.Ki%K_O); 1885 .LM287: 1886 0798 8091 0000 lds r24,R_ParameterTable+4 1887 079c 6AE0 ldi r22,lo8(10) 1888 079e 0E94 0000 call __udivmodqi4 1889 07a2 892F mov r24,r25 1890 07a4 0E94 0000 call PutDecByte 580:srx1.c **** PutChar(' '); 1892 .LM288: 1893 07a8 80E2 ldi r24,lo8(32) 1894 07aa 0E94 0000 call PutChar 581:srx1.c **** PutChar('D'); 1896 .LM289: 1897 07ae 84E4 ldi r24,lo8(68) 1898 07b0 0E94 0000 call PutChar 582:srx1.c **** PutChar('='); 1900 .LM290: 1901 07b4 8DE3 ldi r24,lo8(61) 1902 07b6 0E94 0000 call PutChar 583:srx1.c **** PutDecSignedByte(R_ParameterTable.Kd/K_O); 1904 .LM291: 1905 07ba 8091 0000 lds r24,R_ParameterTable+5 1906 07be 6AE0 ldi r22,lo8(10) 1907 07c0 0E94 0000 call __udivmodqi4 1908 07c4 0E94 0000 call PutDecSignedByte 584:srx1.c **** PutChar('.'); 1910 .LM292: 1911 07c8 8EE2 ldi r24,lo8(46) 1912 07ca 0E94 0000 call PutChar 585:srx1.c **** PutDecByte(R_ParameterTable.Kd%K_O); 1914 .LM293: 1915 07ce 8091 0000 lds r24,R_ParameterTable+5 1916 07d2 6AE0 ldi r22,lo8(10) 1917 07d4 0E94 0000 call __udivmodqi4 1918 07d8 892F mov r24,r25 1919 07da 0E94 0000 call PutDecByte 586:srx1.c **** 587:srx1.c **** // TEMP: Display Follow PD coefficients 588:srx1.c **** PutCRLF(); 1921 .LM294: 1922 07de 0E94 0000 call PutCRLF 589:srx1.c **** PutChar('p'); 1924 .LM295: 1925 07e2 80E7 ldi r24,lo8(112) 1926 07e4 0E94 0000 call PutChar 590:srx1.c **** PutChar('='); 1928 .LM296: 1929 07e8 8DE3 ldi r24,lo8(61) 1930 07ea 0E94 0000 call PutChar 591:srx1.c **** PutDecSignedByte(R_ParameterTable.f_p/F_O); 1932 .LM297: 1933 07ee 8091 0000 lds r24,R_ParameterTable+1 1934 07f2 6AE0 ldi r22,lo8(10) 1935 07f4 0E94 0000 call __udivmodqi4 1936 07f8 0E94 0000 call PutDecSignedByte 592:srx1.c **** PutChar('.'); 1938 .LM298: 1939 07fc 8EE2 ldi r24,lo8(46) 1940 07fe 0E94 0000 call PutChar 593:srx1.c **** PutDecByte(R_ParameterTable.f_p%F_O); 1942 .LM299: 1943 0802 8091 0000 lds r24,R_ParameterTable+1 1944 0806 6AE0 ldi r22,lo8(10) 1945 0808 0E94 0000 call __udivmodqi4 1946 080c 892F mov r24,r25 1947 080e 0E94 0000 call PutDecByte 594:srx1.c **** PutChar(' '); 1949 .LM300: 1950 0812 80E2 ldi r24,lo8(32) 1951 0814 0E94 0000 call PutChar 595:srx1.c **** PutChar('d'); 1953 .LM301: 1954 0818 84E6 ldi r24,lo8(100) 1955 081a 0E94 0000 call PutChar 596:srx1.c **** PutChar('='); 1957 .LM302: 1958 081e 8DE3 ldi r24,lo8(61) 1959 0820 0E94 0000 call PutChar 597:srx1.c **** PutDecSignedByte(R_ParameterTable.f_d/F_O); 1961 .LM303: 1962 0824 8091 0000 lds r24,R_ParameterTable+2 1963 0828 6AE0 ldi r22,lo8(10) 1964 082a 0E94 0000 call __udivmodqi4 1965 082e 0E94 0000 call PutDecSignedByte 598:srx1.c **** PutChar('.'); 1967 .LM304: 1968 0832 8EE2 ldi r24,lo8(46) 1969 0834 0E94 0000 call PutChar 599:srx1.c **** PutDecByte(R_ParameterTable.f_d%F_O); 1971 .LM305: 1972 0838 8091 0000 lds r24,R_ParameterTable+2 1973 083c 6AE0 ldi r22,lo8(10) 1974 083e 0E94 0000 call __udivmodqi4 1975 0842 892F mov r24,r25 1976 0844 0E94 0000 call PutDecByte 600:srx1.c **** 601:srx1.c **** PrintString(str_spacer); 1978 .LM306: 1979 0848 80E0 ldi r24,lo8(str_spacer) 1980 084a 90E0 ldi r25,hi8(str_spacer) 1981 084c 0E94 0000 call PrintString 602:srx1.c **** 603:srx1.c **** // Print Line sensor readings 604:srx1.c **** PrintString(str_line_sensors); 1983 .LM307: 1984 0850 80E0 ldi r24,lo8(str_line_sensors) 1985 0852 90E0 ldi r25,hi8(str_line_sensors) 1986 0854 0E94 0000 call PrintString 605:srx1.c **** PutChar('L'); 1988 .LM308: 1989 0858 8CE4 ldi r24,lo8(76) 1990 085a 0E94 0000 call PutChar 606:srx1.c **** PutChar(':'); 1992 .LM309: 1993 085e 8AE3 ldi r24,lo8(58) 1994 0860 0E94 0000 call PutChar 607:srx1.c **** temp_char = adc_read(LINE_SENSOR_L); 1996 .LM310: 1997 0864 86E0 ldi r24,lo8(6) 1998 0866 0E94 0000 call adc_read 1999 086a 082F mov r16,r24 608:srx1.c **** if (LINE_SENSOR(temp_char)) 2001 .LM311: 2002 086c 8091 0000 lds r24,R_ParameterTable+6 2003 0870 8017 cp r24,r16 2004 0872 10F4 brsh .L77 609:srx1.c **** PutChar('1'); 2006 .LM312: 2007 0874 81E3 ldi r24,lo8(49) 2008 0876 01C0 rjmp .L214 2009 .L77: 610:srx1.c **** else 611:srx1.c **** PutChar('0'); 2011 .LM313: 2012 0878 80E3 ldi r24,lo8(48) 2013 .L214: 2014 087a 0E94 0000 call PutChar 612:srx1.c **** PutChar(' '); 2016 .LM314: 2017 087e 80E2 ldi r24,lo8(32) 2018 0880 0E94 0000 call PutChar 613:srx1.c **** PutChar('('); 2020 .LM315: 2021 0884 88E2 ldi r24,lo8(40) 2022 0886 0E94 0000 call PutChar 614:srx1.c **** PutDecByte(temp_char); 2024 .LM316: 2025 088a 802F mov r24,r16 2026 088c 0E94 0000 call PutDecByte 615:srx1.c **** PutChar(')'); 2028 .LM317: 2029 0890 89E2 ldi r24,lo8(41) 2030 0892 0E94 0000 call PutChar 616:srx1.c **** 617:srx1.c **** PutChar(' '); 2032 .LM318: 2033 0896 80E2 ldi r24,lo8(32) 2034 0898 0E94 0000 call PutChar 618:srx1.c **** PutChar('R'); 2036 .LM319: 2037 089c 82E5 ldi r24,lo8(82) 2038 089e 0E94 0000 call PutChar 619:srx1.c **** PutChar(':'); 2040 .LM320: 2041 08a2 8AE3 ldi r24,lo8(58) 2042 08a4 0E94 0000 call PutChar 620:srx1.c **** temp_char = adc_read(LINE_SENSOR_R); 2044 .LM321: 2045 08a8 87E0 ldi r24,lo8(7) 2046 08aa 0E94 0000 call adc_read 2047 08ae 082F mov r16,r24 621:srx1.c **** if (LINE_SENSOR(temp_char)) 2049 .LM322: 2050 08b0 8091 0000 lds r24,R_ParameterTable+6 2051 08b4 8017 cp r24,r16 2052 08b6 10F4 brsh .L79 622:srx1.c **** PutChar('1'); 2054 .LM323: 2055 08b8 81E3 ldi r24,lo8(49) 2056 08ba 01C0 rjmp .L215 2057 .L79: 623:srx1.c **** else 624:srx1.c **** PutChar('0'); 2059 .LM324: 2060 08bc 80E3 ldi r24,lo8(48) 2061 .L215: 2062 08be 0E94 0000 call PutChar 625:srx1.c **** PutChar(' '); 2064 .LM325: 2065 08c2 80E2 ldi r24,lo8(32) 2066 08c4 0E94 0000 call PutChar 626:srx1.c **** PutChar('('); 2068 .LM326: 2069 08c8 88E2 ldi r24,lo8(40) 2070 08ca 0E94 0000 call PutChar 627:srx1.c **** PutDecByte(temp_char); 2072 .LM327: 2073 08ce 802F mov r24,r16 2074 08d0 0E94 0000 call PutDecByte 628:srx1.c **** PutChar(')'); 2076 .LM328: 2077 08d4 89E2 ldi r24,lo8(41) 2078 08d6 0E94 0000 call PutChar 629:srx1.c **** 630:srx1.c **** PrintString(str_spacer); 2080 .LM329: 2081 08da 80E0 ldi r24,lo8(str_spacer) 2082 08dc 90E0 ldi r25,hi8(str_spacer) 2083 08de 0E94 0000 call PrintString 631:srx1.c **** 632:srx1.c **** // Print GP2D12 sensors readings 633:srx1.c **** PrintString(str_gpd_readings); 2085 .LM330: 2086 08e2 80E0 ldi r24,lo8(str_gpd_readings) 2087 08e4 90E0 ldi r25,hi8(str_gpd_readings) 2088 08e6 0E94 0000 call PrintString 634:srx1.c **** PutChar('\t'); 2090 .LM331: 2091 08ea 89E0 ldi r24,lo8(9) 2092 08ec 0E94 0000 call PutChar 635:srx1.c **** PutChar('\t'); 2094 .LM332: 2095 08f0 89E0 ldi r24,lo8(9) 2096 08f2 0E94 0000 call PutChar 636:srx1.c **** PutDecByte(gpd_read(GPD_F)); 2098 .LM333: 2099 08f6 80E0 ldi r24,lo8(0) 2100 08f8 0E94 0000 call gpd_read 2101 08fc 0E94 0000 call PutDecByte 637:srx1.c **** PutCRLF(); 2103 .LM334: 2104 0900 0E94 0000 call PutCRLF 638:srx1.c **** PutChar('\t'); 2106 .LM335: 2107 0904 89E0 ldi r24,lo8(9) 2108 0906 0E94 0000 call PutChar 639:srx1.c **** PutChar('\t'); 2110 .LM336: 2111 090a 89E0 ldi r24,lo8(9) 2112 090c 0E94 0000 call PutChar 640:srx1.c **** PutChar('\t'); 2114 .LM337: 2115 0910 89E0 ldi r24,lo8(9) 2116 0912 0E94 0000 call PutChar 641:srx1.c **** PutDecByte(gpd_read(GPD_L_F)); 2118 .LM338: 2119 0916 81E0 ldi r24,lo8(1) 2120 0918 0E94 0000 call gpd_read 2121 091c 0E94 0000 call PutDecByte 642:srx1.c **** PutChar('\t'); 2123 .LM339: 2124 0920 89E0 ldi r24,lo8(9) 2125 0922 0E94 0000 call PutChar 643:srx1.c **** PutChar('\t'); 2127 .LM340: 2128 0926 89E0 ldi r24,lo8(9) 2129 0928 0E94 0000 call PutChar 644:srx1.c **** PutDecByte(gpd_read(GPD_R_F)); 2131 .LM341: 2132 092c 83E0 ldi r24,lo8(3) 2133 092e 0E94 0000 call gpd_read 2134 0932 0E94 0000 call PutDecByte 645:srx1.c **** PutCRLF(); 2136 .LM342: 2137 0936 0E94 0000 call PutCRLF 646:srx1.c **** PutChar('\t'); 2139 .LM343: 2140 093a 89E0 ldi r24,lo8(9) 2141 093c 0E94 0000 call PutChar 647:srx1.c **** PutChar('\t'); 2143 .LM344: 2144 0940 89E0 ldi r24,lo8(9) 2145 0942 0E94 0000 call PutChar 648:srx1.c **** PutChar('\t'); 2147 .LM345: 2148 0946 89E0 ldi r24,lo8(9) 2149 0948 0E94 0000 call PutChar 649:srx1.c **** PutDecByte(gpd_read(GPD_L_B)); 2151 .LM346: 2152 094c 82E0 ldi r24,lo8(2) 2153 094e 0E94 0000 call gpd_read 2154 0952 0E94 0000 call PutDecByte 650:srx1.c **** PutChar('\t'); 2156 .LM347: 2157 0956 89E0 ldi r24,lo8(9) 2158 0958 0E94 0000 call PutChar 651:srx1.c **** PutChar('\t'); 2160 .LM348: 2161 095c 89E0 ldi r24,lo8(9) 2162 095e 0E94 0000 call PutChar 652:srx1.c **** PutDecByte(gpd_read(GPD_R_B)); 2164 .LM349: 2165 0962 84E0 ldi r24,lo8(4) 2166 0964 0E94 0000 call gpd_read 2167 0968 0E94 0000 call PutDecByte 653:srx1.c **** 654:srx1.c **** // Display SCM status 655:srx1.c **** PrintString(str_spacer); 2169 .LM350: 2170 096c 80E0 ldi r24,lo8(str_spacer) 2171 096e 90E0 ldi r25,hi8(str_spacer) 2172 0970 0E94 0000 call PrintString 656:srx1.c **** PutCRLF(); 2174 .LM351: 2175 0974 0E94 0000 call PutCRLF 657:srx1.c **** 658:srx1.c **** PutChar('S'); 2177 .LM352: 2178 0978 83E5 ldi r24,lo8(83) 2179 097a 0E94 0000 call PutChar 659:srx1.c **** PutChar('C'); 2181 .LM353: 2182 097e 83E4 ldi r24,lo8(67) 2183 0980 0E94 0000 call PutChar 660:srx1.c **** PutChar('M'); 2185 .LM354: 2186 0984 8DE4 ldi r24,lo8(77) 2187 0986 0E94 0000 call PutChar 661:srx1.c **** PutChar(':'); 2189 .LM355: 2190 098a 8AE3 ldi r24,lo8(58) 2191 098c 0E94 0000 call PutChar 662:srx1.c **** PutChar(' '); 2193 .LM356: 2194 0990 80E2 ldi r24,lo8(32) 2195 0992 0E94 0000 call PutChar 663:srx1.c **** 664:srx1.c **** if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS)) 2197 .LM357: 2198 0996 62E0 ldi r22,lo8(2) 2199 0998 80EB ldi r24,lo8(-80) 2200 099a 0E94 0000 call i2c_readbyte 2201 099e 8823 tst r24 2202 09a0 A1F0 breq .L81 665:srx1.c **** { 666:srx1.c **** PutChar('('); 2204 .LM358: 2205 09a2 88E2 ldi r24,lo8(40) 2206 09a4 0E94 0000 call PutChar 667:srx1.c **** PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_X)); 2208 .LM359: 2209 09a8 64E0 ldi r22,lo8(4) 2210 09aa 80EB ldi r24,lo8(-80) 2211 09ac 0E94 0000 call i2c_readbyte 2212 09b0 0E94 0000 call PutDecByte 668:srx1.c **** PutChar(','); 2214 .LM360: 2215 09b4 8CE2 ldi r24,lo8(44) 2216 09b6 0E94 0000 call PutChar 669:srx1.c **** PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_Y)); 2218 .LM361: 2219 09ba 65E0 ldi r22,lo8(5) 2220 09bc 80EB ldi r24,lo8(-80) 2221 09be 0E94 0000 call i2c_readbyte 2222 09c2 0E94 0000 call PutDecByte 670:srx1.c **** PutChar(')'); 2224 .LM362: 2225 09c6 89E2 ldi r24,lo8(41) 2226 09c8 0AC0 rjmp .L216 2227 .L81: 671:srx1.c **** } 672:srx1.c **** else 673:srx1.c **** { 674:srx1.c **** PutChar('N'); 2229 .LM363: 2230 09ca 8EE4 ldi r24,lo8(78) 2231 09cc 0E94 0000 call PutChar 675:srx1.c **** PutChar('o'); 2233 .LM364: 2234 09d0 8FE6 ldi r24,lo8(111) 2235 09d2 0E94 0000 call PutChar 676:srx1.c **** PutChar('n'); 2237 .LM365: 2238 09d6 8EE6 ldi r24,lo8(110) 2239 09d8 0E94 0000 call PutChar 677:srx1.c **** PutChar('e'); 2241 .LM366: 2242 09dc 85E6 ldi r24,lo8(101) 2243 .L216: 2244 09de 0E94 0000 call PutChar 678:srx1.c **** } 679:srx1.c **** 680:srx1.c **** // Debug - line sensor last triggered 681:srx1.c **** //PutCRLF(); 682:srx1.c **** //PutDecWord(last_line_trigger); 683:srx1.c **** 684:srx1.c **** // Tidy up 685:srx1.c **** PrintString(str_exec_login_bar); 2246 .LM367: 2247 09e2 80E0 ldi r24,lo8(str_exec_login_bar) 2248 09e4 90E0 ldi r25,hi8(str_exec_login_bar) 2249 09e6 CEC0 rjmp .L223 2250 .L83: 686:srx1.c **** break; 687:srx1.c **** 688:srx1.c **** // Stop motors 689:srx1.c **** case ('S'): 690:srx1.c **** ArbitrateCmd.info.output_l = POWER(0); 2252 .LM368: 2253 09e8 1092 0000 sts (ArbitrateCmd.3+8)+1,__zero_reg__ 2254 09ec 1092 0000 sts ArbitrateCmd.3+8,__zero_reg__ 691:srx1.c **** ArbitrateCmd.info.output_r = POWER(0); 2256 .LM369: 2257 09f0 1092 0000 sts (ArbitrateCmd.3+10)+1,__zero_reg__ 2258 09f4 1092 0000 sts ArbitrateCmd.3+10,__zero_reg__ 692:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2260 .LM370: 2261 09f8 6FC3 rjmp .L222 2262 .L84: 693:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 694:srx1.c **** break; 695:srx1.c **** 696:srx1.c **** // Rotate Right 697:srx1.c **** case ('D'): 698:srx1.c **** ArbitrateCmd.info.output_r = POWER(-50); 2264 .LM371: 2265 09fa 8CEE ldi r24,lo8(-20) 2266 09fc 9FEF ldi r25,hi8(-20) 2267 09fe 9093 0000 sts (ArbitrateCmd.3+10)+1,r25 2268 0a02 8093 0000 sts ArbitrateCmd.3+10,r24 699:srx1.c **** ArbitrateCmd.info.output_l = POWER(50); 2270 .LM372: 2271 0a06 84E1 ldi r24,lo8(20) 2272 0a08 90E0 ldi r25,hi8(20) 2273 0a0a 08C0 rjmp .L226 2274 .L85: 700:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 701:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 702:srx1.c **** break; 703:srx1.c **** 704:srx1.c **** // Rotate Left 705:srx1.c **** case ('A'): 706:srx1.c **** ArbitrateCmd.info.output_r = POWER(50); 2276 .LM373: 2277 0a0c 84E1 ldi r24,lo8(20) 2278 0a0e 90E0 ldi r25,hi8(20) 2279 0a10 9093 0000 sts (ArbitrateCmd.3+10)+1,r25 2280 0a14 8093 0000 sts ArbitrateCmd.3+10,r24 707:srx1.c **** ArbitrateCmd.info.output_l = POWER(-50); 2282 .LM374: 2283 0a18 8CEE ldi r24,lo8(-20) 2284 0a1a 9FEF ldi r25,hi8(-20) 2285 .L226: 2286 0a1c 9093 0000 sts (ArbitrateCmd.3+8)+1,r25 2287 0a20 8093 0000 sts ArbitrateCmd.3+8,r24 708:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2289 .LM375: 2290 0a24 59C3 rjmp .L222 2291 .L86: 709:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 710:srx1.c **** break; 711:srx1.c **** 712:srx1.c **** // Go forward at High speed 713:srx1.c **** case ('3'): 714:srx1.c **** ArbitrateCmd.info.output_r = POWER(90); 2293 .LM376: 2294 0a26 84E2 ldi r24,lo8(36) 2295 0a28 90E0 ldi r25,hi8(36) 2296 0a2a 0BC0 rjmp .L227 2297 .L87: 715:srx1.c **** ArbitrateCmd.info.output_l = POWER(90); 716:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 717:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 718:srx1.c **** break; 719:srx1.c **** 720:srx1.c **** // Go forward at medium speed 721:srx1.c **** case ('2'): 722:srx1.c **** ArbitrateCmd.info.output_r = POWER(56); 2299 .LM377: 2300 0a2c 86E1 ldi r24,lo8(22) 2301 0a2e 90E0 ldi r25,hi8(22) 2302 0a30 08C0 rjmp .L227 2303 .L88: 723:srx1.c **** ArbitrateCmd.info.output_l = POWER(56); 724:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 725:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 726:srx1.c **** break; 727:srx1.c **** 728:srx1.c **** // Go forward at low speed 729:srx1.c **** case ('1'): 730:srx1.c **** ArbitrateCmd.info.output_r = POWER(36); 2305 .LM378: 2306 0a32 8EE0 ldi r24,lo8(14) 2307 0a34 90E0 ldi r25,hi8(14) 2308 0a36 05C0 rjmp .L227 2309 .L89: 731:srx1.c **** ArbitrateCmd.info.output_l = POWER(36); 732:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 733:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 734:srx1.c **** break; 735:srx1.c **** 736:srx1.c **** // Go Backwards at medium speed 737:srx1.c **** case ('5'): 738:srx1.c **** ArbitrateCmd.info.output_r = POWER(-56); 2311 .LM379: 2312 0a38 8AEE ldi r24,lo8(-22) 2313 0a3a 9FEF ldi r25,hi8(-22) 2314 0a3c 02C0 rjmp .L227 2315 .L90: 739:srx1.c **** ArbitrateCmd.info.output_l = POWER(-56); 740:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 741:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 742:srx1.c **** break; 743:srx1.c **** 744:srx1.c **** // Go Backwards at low speed 745:srx1.c **** case ('4'): 746:srx1.c **** ArbitrateCmd.info.output_r = POWER(-35); 2317 .LM380: 2318 0a3e 82EF ldi r24,lo8(-14) 2319 0a40 9FEF ldi r25,hi8(-14) 2320 .L227: 2321 0a42 9093 0000 sts (ArbitrateCmd.3+10)+1,r25 2322 0a46 8093 0000 sts ArbitrateCmd.3+10,r24 747:srx1.c **** ArbitrateCmd.info.output_l = POWER(-35); 2324 .LM381: 2325 0a4a E8CF rjmp .L226 2326 .L91: 748:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 749:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 750:srx1.c **** break; 751:srx1.c **** 752:srx1.c **** // Read an I/O port 753:srx1.c **** case ('r'): 754:srx1.c **** PrintString(str_enter_port_letter); 2328 .LM382: 2329 0a4c 80E0 ldi r24,lo8(str_enter_port_letter) 2330 0a4e 90E0 ldi r25,hi8(str_enter_port_letter) 2331 0a50 0E94 0000 call PrintString 755:srx1.c **** temp_char = _GetChar(); 2333 .LM383: 2334 0a54 0E94 0000 call _GetChar 756:srx1.c **** 757:srx1.c **** switch(temp_char) 2336 .LM384: 2337 0a58 682F mov r22,r24 2338 0a5a 7727 clr r23 2339 0a5c 6534 cpi r22,69 2340 0a5e 7105 cpc r23,__zero_reg__ 2341 0a60 09F4 brne .+2 2342 0a62 77C0 rjmp .L102 2343 0a64 6634 cpi r22,70 2344 0a66 7105 cpc r23,__zero_reg__ 2345 0a68 9CF4 brge .L105 2346 0a6a 6234 cpi r22,66 2347 0a6c 7105 cpc r23,__zero_reg__ 2348 0a6e C1F1 breq .L96 2349 0a70 6334 cpi r22,67 2350 0a72 7105 cpc r23,__zero_reg__ 2351 0a74 24F4 brge .L106 2352 0a76 6134 cpi r22,65 2353 0a78 7105 cpc r23,__zero_reg__ 2354 0a7a F9F0 breq .L94 2355 0a7c 3AC3 rjmp .L11 2356 .L106: 2357 0a7e 6334 cpi r22,67 2358 0a80 7105 cpc r23,__zero_reg__ 2359 0a82 09F4 brne .+2 2360 0a84 40C0 rjmp .L98 2361 0a86 6434 cpi r22,68 2362 0a88 7105 cpc r23,__zero_reg__ 2363 0a8a 09F4 brne .+2 2364 0a8c 4FC0 rjmp .L100 2365 0a8e 31C3 rjmp .L11 2366 .L105: 2367 0a90 6336 cpi r22,99 2368 0a92 7105 cpc r23,__zero_reg__ 2369 0a94 C1F1 breq .L98 2370 0a96 6436 cpi r22,100 2371 0a98 7105 cpc r23,__zero_reg__ 2372 0a9a 3CF4 brge .L107 2373 0a9c 6136 cpi r22,97 2374 0a9e 7105 cpc r23,__zero_reg__ 2375 0aa0 61F0 breq .L94 2376 0aa2 6236 cpi r22,98 2377 0aa4 7105 cpc r23,__zero_reg__ 2378 0aa6 E1F0 breq .L96 2379 0aa8 24C3 rjmp .L11 2380 .L107: 2381 0aaa 6436 cpi r22,100 2382 0aac 7105 cpc r23,__zero_reg__ 2383 0aae F1F1 breq .L100 2384 0ab0 6536 cpi r22,101 2385 0ab2 7105 cpc r23,__zero_reg__ 2386 0ab4 09F4 brne .+2 2387 0ab6 4DC0 rjmp .L102 2388 0ab8 1CC3 rjmp .L11 2389 .L94: 758:srx1.c **** { 759:srx1.c **** case ('A'): 760:srx1.c **** case ('a'): 761:srx1.c **** PrintString(str_port_read_banner); 2391 .LM385: 2392 0aba 80E0 ldi r24,lo8(str_port_read_banner) 2393 0abc 90E0 ldi r25,hi8(str_port_read_banner) 2394 0abe 0E94 0000 call PrintString 762:srx1.c **** PutChar('A'); 2396 .LM386: 2397 0ac2 81E4 ldi r24,lo8(65) 2398 0ac4 0E94 0000 call PutChar 763:srx1.c **** PutChar('='); 2400 .LM387: 2401 0ac8 8DE3 ldi r24,lo8(61) 2402 0aca 0E94 0000 call PutChar 764:srx1.c **** PutHexByte(inp(PINA)); 2404 .LM388: 2405 0ace 89B3 in r24,57-0x20 2406 0ad0 0E94 0000 call PutHexByte 765:srx1.c **** PrintString(str_port_read_banner2); 2408 .LM389: 2409 0ad4 80E0 ldi r24,lo8(str_port_read_banner2) 2410 0ad6 90E0 ldi r25,hi8(str_port_read_banner2) 2411 0ad8 0E94 0000 call PrintString 766:srx1.c **** PutBin(inp(PINA)); 2413 .LM390: 2414 0adc 89B3 in r24,57-0x20 2415 0ade 4BC0 rjmp .L221 2416 .L96: 767:srx1.c **** break; 768:srx1.c **** 769:srx1.c **** case ('B'): 770:srx1.c **** case ('b'): 771:srx1.c **** PrintString(str_port_read_banner); 2418 .LM391: 2419 0ae0 80E0 ldi r24,lo8(str_port_read_banner) 2420 0ae2 90E0 ldi r25,hi8(str_port_read_banner) 2421 0ae4 0E94 0000 call PrintString 772:srx1.c **** PutChar('B'); 2423 .LM392: 2424 0ae8 82E4 ldi r24,lo8(66) 2425 0aea 0E94 0000 call PutChar 773:srx1.c **** PutChar('='); 2427 .LM393: 2428 0aee 8DE3 ldi r24,lo8(61) 2429 0af0 0E94 0000 call PutChar 774:srx1.c **** PutHexByte(inp(PINB)); 2431 .LM394: 2432 0af4 86B3 in r24,54-0x20 2433 0af6 0E94 0000 call PutHexByte 775:srx1.c **** PrintString(str_port_read_banner2); 2435 .LM395: 2436 0afa 80E0 ldi r24,lo8(str_port_read_banner2) 2437 0afc 90E0 ldi r25,hi8(str_port_read_banner2) 2438 0afe 0E94 0000 call PrintString 776:srx1.c **** PutBin(inp(PINB)); 2440 .LM396: 2441 0b02 86B3 in r24,54-0x20 2442 0b04 38C0 rjmp .L221 2443 .L98: 777:srx1.c **** break; 778:srx1.c **** 779:srx1.c **** // Special case: This is an output port only so print current output 780:srx1.c **** case ('C'): 781:srx1.c **** case ('c'): 782:srx1.c **** PrintString(str_port_read_banner); 2445 .LM397: 2446 0b06 80E0 ldi r24,lo8(str_port_read_banner) 2447 0b08 90E0 ldi r25,hi8(str_port_read_banner) 2448 0b0a 0E94 0000 call PrintString 783:srx1.c **** PutChar('C'); 2450 .LM398: 2451 0b0e 83E4 ldi r24,lo8(67) 2452 0b10 0E94 0000 call PutChar 784:srx1.c **** PutChar('='); 2454 .LM399: 2455 0b14 8DE3 ldi r24,lo8(61) 2456 0b16 0E94 0000 call PutChar 785:srx1.c **** PutHexByte(inp(PORTC)); 2458 .LM400: 2459 0b1a 85B3 in r24,53-0x20 2460 0b1c 0E94 0000 call PutHexByte 786:srx1.c **** PrintString(str_port_read_banner2); 2462 .LM401: 2463 0b20 80E0 ldi r24,lo8(str_port_read_banner2) 2464 0b22 90E0 ldi r25,hi8(str_port_read_banner2) 2465 0b24 0E94 0000 call PrintString 787:srx1.c **** PutBin(inp(PORTC)); 2467 .LM402: 2468 0b28 85B3 in r24,53-0x20 2469 0b2a 25C0 rjmp .L221 2470 .L100: 788:srx1.c **** break; 789:srx1.c **** case ('D'): 790:srx1.c **** case ('d'): 791:srx1.c **** PrintString(str_port_read_banner); 2472 .LM403: 2473 0b2c 80E0 ldi r24,lo8(str_port_read_banner) 2474 0b2e 90E0 ldi r25,hi8(str_port_read_banner) 2475 0b30 0E94 0000 call PrintString 792:srx1.c **** PutChar('D'); 2477 .LM404: 2478 0b34 84E4 ldi r24,lo8(68) 2479 0b36 0E94 0000 call PutChar 793:srx1.c **** PutChar('='); 2481 .LM405: 2482 0b3a 8DE3 ldi r24,lo8(61) 2483 0b3c 0E94 0000 call PutChar 794:srx1.c **** PutHexByte(inp(PIND)); 2485 .LM406: 2486 0b40 80B3 in r24,48-0x20 2487 0b42 0E94 0000 call PutHexByte 795:srx1.c **** PrintString(str_port_read_banner2); 2489 .LM407: 2490 0b46 80E0 ldi r24,lo8(str_port_read_banner2) 2491 0b48 90E0 ldi r25,hi8(str_port_read_banner2) 2492 0b4a 0E94 0000 call PrintString 796:srx1.c **** PutBin(inp(PIND)); 2494 .LM408: 2495 0b4e 80B3 in r24,48-0x20 2496 0b50 12C0 rjmp .L221 2497 .L102: 797:srx1.c **** break; 798:srx1.c **** 799:srx1.c **** case ('E'): 800:srx1.c **** case ('e'): 801:srx1.c **** PrintString(str_port_read_banner); 2499 .LM409: 2500 0b52 80E0 ldi r24,lo8(str_port_read_banner) 2501 0b54 90E0 ldi r25,hi8(str_port_read_banner) 2502 0b56 0E94 0000 call PrintString 802:srx1.c **** PutChar('E'); 2504 .LM410: 2505 0b5a 85E4 ldi r24,lo8(69) 2506 0b5c 0E94 0000 call PutChar 803:srx1.c **** PutChar('='); 2508 .LM411: 2509 0b60 8DE3 ldi r24,lo8(61) 2510 0b62 0E94 0000 call PutChar 804:srx1.c **** PutHexByte(inp(PINE)); 2512 .LM412: 2513 0b66 81B1 in r24,33-0x20 2514 0b68 0E94 0000 call PutHexByte 805:srx1.c **** PrintString(str_port_read_banner2); 2516 .LM413: 2517 0b6c 80E0 ldi r24,lo8(str_port_read_banner2) 2518 0b6e 90E0 ldi r25,hi8(str_port_read_banner2) 2519 0b70 0E94 0000 call PrintString 806:srx1.c **** PutBin(inp(PINE)); 2521 .LM414: 2522 0b74 81B1 in r24,33-0x20 2523 .L221: 2524 0b76 0E94 0000 call PutBin 807:srx1.c **** break; 2526 .LM415: 2527 0b7a BBC2 rjmp .L11 2528 .L108: 808:srx1.c **** } 809:srx1.c **** break; 810:srx1.c **** 811:srx1.c **** // Quit EXEC mode. 812:srx1.c **** case ('`'): 813:srx1.c **** exit = TRUE; 2530 .LM416: 2531 0b7c 31E0 ldi r19,lo8(1) 2532 0b7e F32E mov r15,r19 814:srx1.c **** PrintString(str_exec_quit); 2534 .LM417: 2535 0b80 80E0 ldi r24,lo8(str_exec_quit) 2536 0b82 90E0 ldi r25,hi8(str_exec_quit) 2537 .L223: 2538 0b84 0E94 0000 call PrintString 815:srx1.c **** break; 2540 .LM418: 2541 0b88 B4C2 rjmp .L11 2542 .L109: 816:srx1.c **** 817:srx1.c **** // Software Reset processor (WARM BOOT) 818:srx1.c **** case ('!'): 819:srx1.c **** PrintString(str_soft_reset); 2544 .LM419: 2545 0b8a 80E0 ldi r24,lo8(str_soft_reset) 2546 0b8c 90E0 ldi r25,hi8(str_soft_reset) 2547 0b8e 0E94 0000 call PrintString 820:srx1.c **** RESET(); 2549 .LM420: 2550 .LBB3: 2551 /* #APP */ 2552 0b92 F894 cli 2553 2554 /* #NOAPP */ 2555 0b94 0E94 0000 call 0 821:srx1.c **** break; 2557 .LM421: 2558 .LBE3: 2559 0b98 ACC2 rjmp .L11 2560 .L110: 822:srx1.c **** 823:srx1.c **** // I2C Byte Read 824:srx1.c **** case ('#'): 825:srx1.c **** PrintString(str_enter_address); 2562 .LM422: 2563 0b9a 80E0 ldi r24,lo8(str_enter_address) 2564 0b9c 90E0 ldi r25,hi8(str_enter_address) 2565 0b9e 0E94 0000 call PrintString 826:srx1.c **** temp_char = GetHexByte(); 2567 .LM423: 2568 0ba2 0E94 0000 call GetHexByte 2569 0ba6 082F mov r16,r24 827:srx1.c **** PutCRLF(); 2571 .LM424: 2572 0ba8 0E94 0000 call PutCRLF 828:srx1.c **** PrintString(str_enter_element); 2574 .LM425: 2575 0bac 80E0 ldi r24,lo8(str_enter_element) 2576 0bae 90E0 ldi r25,hi8(str_enter_element) 2577 0bb0 0E94 0000 call PrintString 829:srx1.c **** temp_char2 = GetHexByte(); 2579 .LM426: 2580 0bb4 0E94 0000 call GetHexByte 2581 0bb8 C82F mov r28,r24 830:srx1.c **** PutCRLF(); 2583 .LM427: 2584 0bba 0E94 0000 call PutCRLF 831:srx1.c **** temp_char = i2c_readbyte(temp_char, temp_char2); 2586 .LM428: 2587 0bbe 6C2F mov r22,r28 2588 0bc0 802F mov r24,r16 2589 0bc2 0E94 0000 call i2c_readbyte 832:srx1.c **** PutHexByte(temp_char); 2591 .LM429: 2592 0bc6 0E94 0000 call PutHexByte 833:srx1.c **** break; 2594 .LM430: 2595 0bca 93C2 rjmp .L11 2596 .L111: 834:srx1.c **** 835:srx1.c **** // I2C Byte Write 836:srx1.c **** case ('$'): 837:srx1.c **** PrintString(str_enter_address); 2598 .LM431: 2599 0bcc 80E0 ldi r24,lo8(str_enter_address) 2600 0bce 90E0 ldi r25,hi8(str_enter_address) 2601 0bd0 0E94 0000 call PrintString 838:srx1.c **** index = GetHexByte(); 2603 .LM432: 2604 0bd4 0E94 0000 call GetHexByte 2605 0bd8 182F mov r17,r24 839:srx1.c **** PutCRLF(); 2607 .LM433: 2608 0bda 0E94 0000 call PutCRLF 840:srx1.c **** PrintString(str_enter_element); 2610 .LM434: 2611 0bde 80E0 ldi r24,lo8(str_enter_element) 2612 0be0 90E0 ldi r25,hi8(str_enter_element) 2613 0be2 0E94 0000 call PrintString 841:srx1.c **** temp_char = GetHexByte(); 2615 .LM435: 2616 0be6 0E94 0000 call GetHexByte 2617 0bea 082F mov r16,r24 842:srx1.c **** PutCRLF(); 2619 .LM436: 2620 0bec 0E94 0000 call PutCRLF 843:srx1.c **** PrintString(str_enter_byte); 2622 .LM437: 2623 0bf0 80E0 ldi r24,lo8(str_enter_byte) 2624 0bf2 90E0 ldi r25,hi8(str_enter_byte) 2625 0bf4 0E94 0000 call PrintString 844:srx1.c **** temp_char2 = GetHexByte(); 2627 .LM438: 2628 0bf8 0E94 0000 call GetHexByte 845:srx1.c **** i2c_writebyte(index, temp_char, temp_char2); 2630 .LM439: 2631 0bfc 482F mov r20,r24 2632 0bfe 602F mov r22,r16 2633 0c00 812F mov r24,r17 2634 0c02 0E94 0000 call i2c_writebyte 846:srx1.c **** break; 2636 .LM440: 2637 0c06 75C2 rjmp .L11 2638 .L112: 847:srx1.c **** 848:srx1.c **** // I2C Word Read 849:srx1.c **** case ('%'): 850:srx1.c **** PrintString(str_enter_address); 2640 .LM441: 2641 0c08 80E0 ldi r24,lo8(str_enter_address) 2642 0c0a 90E0 ldi r25,hi8(str_enter_address) 2643 0c0c 0E94 0000 call PrintString 851:srx1.c **** temp_char = GetHexByte(); 2645 .LM442: 2646 0c10 0E94 0000 call GetHexByte 2647 0c14 082F mov r16,r24 852:srx1.c **** PutCRLF(); 2649 .LM443: 2650 0c16 0E94 0000 call PutCRLF 853:srx1.c **** PrintString(str_enter_element); 2652 .LM444: 2653 0c1a 80E0 ldi r24,lo8(str_enter_element) 2654 0c1c 90E0 ldi r25,hi8(str_enter_element) 2655 0c1e 0E94 0000 call PrintString 854:srx1.c **** temp_char2 = GetHexByte(); 2657 .LM445: 2658 0c22 0E94 0000 call GetHexByte 2659 0c26 C82F mov r28,r24 855:srx1.c **** PutCRLF(); 2661 .LM446: 2662 0c28 0E94 0000 call PutCRLF 856:srx1.c **** temp_int = i2c_readword(temp_char, temp_char2); 2664 .LM447: 2665 0c2c 6C2F mov r22,r28 2666 0c2e 802F mov r24,r16 2667 0c30 0E94 0000 call i2c_readword 857:srx1.c **** PutHexWord(temp_int); 2669 .LM448: 2670 0c34 0E94 0000 call PutHexWord 858:srx1.c **** break; 2672 .LM449: 2673 0c38 5CC2 rjmp .L11 2674 .L113: 859:srx1.c **** 860:srx1.c **** // I2C Word Write 861:srx1.c **** case ('^'): 862:srx1.c **** PrintString(str_enter_address); 2676 .LM450: 2677 0c3a 80E0 ldi r24,lo8(str_enter_address) 2678 0c3c 90E0 ldi r25,hi8(str_enter_address) 2679 0c3e 0E94 0000 call PrintString 863:srx1.c **** index = GetHexByte(); 2681 .LM451: 2682 0c42 0E94 0000 call GetHexByte 2683 0c46 182F mov r17,r24 864:srx1.c **** PutCRLF(); 2685 .LM452: 2686 0c48 0E94 0000 call PutCRLF 865:srx1.c **** PrintString(str_enter_element); 2688 .LM453: 2689 0c4c 80E0 ldi r24,lo8(str_enter_element) 2690 0c4e 90E0 ldi r25,hi8(str_enter_element) 2691 0c50 0E94 0000 call PrintString 866:srx1.c **** temp_char = GetHexByte(); 2693 .LM454: 2694 0c54 0E94 0000 call GetHexByte 2695 0c58 082F mov r16,r24 867:srx1.c **** PutCRLF(); 2697 .LM455: 2698 0c5a 0E94 0000 call PutCRLF 868:srx1.c **** PrintString(str_enter_byte); 2700 .LM456: 2701 0c5e 80E0 ldi r24,lo8(str_enter_byte) 2702 0c60 90E0 ldi r25,hi8(str_enter_byte) 2703 0c62 0E94 0000 call PrintString 869:srx1.c **** temp_int = GetHexWord(); 2705 .LM457: 2706 0c66 0E94 0000 call GetHexWord 2707 0c6a AC01 movw r20,r24 870:srx1.c **** i2c_writeword(index, temp_char, temp_int); 2709 .LM458: 2710 0c6c 602F mov r22,r16 2711 0c6e 812F mov r24,r17 2712 0c70 0E94 0000 call i2c_writeword 871:srx1.c **** break; 2714 .LM459: 2715 0c74 3EC2 rjmp .L11 2716 .L114: 872:srx1.c **** 873:srx1.c **** // Rotate 874:srx1.c **** case ('R'): 875:srx1.c **** PutChar(':'); 2718 .LM460: 2719 0c76 8AE3 ldi r24,lo8(58) 2720 0c78 0E94 0000 call PutChar 2721 .L115: 876:srx1.c **** do 877:srx1.c **** temp_char = GetChar(); 2723 .LM461: 2724 0c7c 0E94 0000 call GetChar 2725 0c80 082F mov r16,r24 2726 0c82 8134 cpi r24,lo8(65) 2727 0c84 11F0 breq .L116 2728 0c86 8235 cpi r24,lo8(82) 2729 0c88 C9F7 brne .L115 2730 .L116: 878:srx1.c **** while (temp_char != 'A' && temp_char != 'R'); 879:srx1.c **** PutChar(temp_char); 2732 .LM462: 2733 0c8a 802F mov r24,r16 2734 0c8c 0E94 0000 call PutChar 880:srx1.c **** if (temp_char == 'R') 2736 .LM463: 2737 0c90 0235 cpi r16,lo8(82) 2738 0c92 19F4 brne .L120 881:srx1.c **** RotateCmd.type = RELATIVE; 2740 .LM464: 2741 0c94 1092 0000 sts RotateCmd.0+6,__zero_reg__ 2742 0c98 03C0 rjmp .L121 2743 .L120: 882:srx1.c **** else 883:srx1.c **** RotateCmd.type = ABSOLUTE; 2745 .LM465: 2746 0c9a 81E0 ldi r24,lo8(1) 2747 0c9c 8093 0000 sts RotateCmd.0+6,r24 2748 .L121: 884:srx1.c **** RotateCmd.scan = FALSE; 2750 .LM466: 2751 0ca0 1092 0000 sts RotateCmd.0+7,__zero_reg__ 885:srx1.c **** PutChar(':'); 2753 .LM467: 2754 0ca4 8AE3 ldi r24,lo8(58) 2755 0ca6 0E94 0000 call PutChar 886:srx1.c **** temp_int = GetDecSignedWord(); 2757 .LM468: 2758 0caa 0E94 0000 call GetDecSignedWord 887:srx1.c **** RotateCmd.angle = temp_int; 2760 .LM469: 2761 0cae 9093 0000 sts (RotateCmd.0+4)+1,r25 2762 0cb2 8093 0000 sts RotateCmd.0+4,r24 888:srx1.c **** 889:srx1.c **** // Temporary disable EXEC's status 890:srx1.c **** ArbitrateCmd.info.active = FALSE; 2764 .LM470: 2765 0cb6 1092 0000 sts ArbitrateCmd.3+6,__zero_reg__ 891:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2767 .LM471: 2768 0cba 60E0 ldi r22,lo8(ArbitrateCmd.3) 2769 0cbc 70E0 ldi r23,hi8(ArbitrateCmd.3) 2770 0cbe 80E0 ldi r24,lo8(ArbitrateQueue) 2771 0cc0 90E0 ldi r25,hi8(ArbitrateQueue) 2772 0cc2 0E94 0000 call AvrXSendMessage 892:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2774 .LM472: 2775 0cc6 80E0 ldi r24,lo8(ArbitrateCmd.3) 2776 0cc8 90E0 ldi r25,hi8(ArbitrateCmd.3) 2777 0cca 0E94 0000 call AvrXWaitMessageAck 893:srx1.c **** 894:srx1.c **** AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); 2779 .LM473: 2780 0cce 60E0 ldi r22,lo8(RotateCmd.0) 2781 0cd0 70E0 ldi r23,hi8(RotateCmd.0) 2782 0cd2 80E0 ldi r24,lo8(RotateQueue) 2783 0cd4 90E0 ldi r25,hi8(RotateQueue) 2784 0cd6 0E94 0000 call AvrXSendMessage 895:srx1.c **** AvrXWaitMessageAck(&RotateCmd.mcb); 2786 .LM474: 2787 0cda 80E0 ldi r24,lo8(RotateCmd.0) 2788 0cdc 90E0 ldi r25,hi8(RotateCmd.0) 2789 0cde F9C0 rjmp .L225 2790 .L122: 896:srx1.c **** 897:srx1.c **** // restore our status 898:srx1.c **** ArbitrateCmd.info.active = TRUE; 899:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 900:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 901:srx1.c **** 902:srx1.c **** break; 903:srx1.c **** 904:srx1.c **** // Go Forward 905:srx1.c **** case ('F'): 906:srx1.c **** PutChar(':'); 2792 .LM475: 2793 0ce0 8AE3 ldi r24,lo8(58) 2794 0ce2 0E94 0000 call PutChar 907:srx1.c **** temp_int = GetDecSignedWord(); 2796 .LM476: 2797 0ce6 0E94 0000 call GetDecSignedWord 908:srx1.c **** ForwardCmd.lenght = temp_int; 2799 .LM477: 2800 0cea 9093 0000 sts (ForwardCmd.1+4)+1,r25 2801 0cee 8093 0000 sts ForwardCmd.1+4,r24 909:srx1.c **** ForwardCmd.ignore_door = FALSE; 2803 .LM478: 2804 0cf2 1092 0000 sts ForwardCmd.1+6,__zero_reg__ 910:srx1.c **** 911:srx1.c **** // Temporary disable EXEC's status 912:srx1.c **** ArbitrateCmd.info.active = FALSE; 2806 .LM479: 2807 0cf6 1092 0000 sts ArbitrateCmd.3+6,__zero_reg__ 913:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2809 .LM480: 2810 0cfa 60E0 ldi r22,lo8(ArbitrateCmd.3) 2811 0cfc 70E0 ldi r23,hi8(ArbitrateCmd.3) 2812 0cfe 80E0 ldi r24,lo8(ArbitrateQueue) 2813 0d00 90E0 ldi r25,hi8(ArbitrateQueue) 2814 0d02 0E94 0000 call AvrXSendMessage 914:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2816 .LM481: 2817 0d06 80E0 ldi r24,lo8(ArbitrateCmd.3) 2818 0d08 90E0 ldi r25,hi8(ArbitrateCmd.3) 2819 0d0a 0E94 0000 call AvrXWaitMessageAck 915:srx1.c **** 916:srx1.c **** AvrXSendMessage(&ForwardQueue, &ForwardCmd.mcb); 2821 .LM482: 2822 0d0e 60E0 ldi r22,lo8(ForwardCmd.1) 2823 0d10 70E0 ldi r23,hi8(ForwardCmd.1) 2824 0d12 80E0 ldi r24,lo8(ForwardQueue) 2825 0d14 90E0 ldi r25,hi8(ForwardQueue) 2826 0d16 0E94 0000 call AvrXSendMessage 917:srx1.c **** AvrXWaitMessageAck(&ForwardCmd.mcb); 2828 .LM483: 2829 0d1a 80E0 ldi r24,lo8(ForwardCmd.1) 2830 0d1c 90E0 ldi r25,hi8(ForwardCmd.1) 2831 0d1e 0E94 0000 call AvrXWaitMessageAck 918:srx1.c **** 919:srx1.c **** // restore our status 920:srx1.c **** ArbitrateCmd.info.active = TRUE; 2833 .LM484: 2834 0d22 81E0 ldi r24,lo8(1) 2835 0d24 8093 0000 sts ArbitrateCmd.3+6,r24 921:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2837 .LM485: 2838 0d28 60E0 ldi r22,lo8(ArbitrateCmd.3) 2839 0d2a 70E0 ldi r23,hi8(ArbitrateCmd.3) 2840 0d2c 80E0 ldi r24,lo8(ArbitrateQueue) 2841 0d2e 90E0 ldi r25,hi8(ArbitrateQueue) 2842 0d30 0E94 0000 call AvrXSendMessage 922:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2844 .LM486: 2845 0d34 80E0 ldi r24,lo8(ArbitrateCmd.3) 2846 0d36 90E0 ldi r25,hi8(ArbitrateCmd.3) 2847 0d38 0E94 0000 call AvrXWaitMessageAck 923:srx1.c **** 924:srx1.c **** // Print reported result 925:srx1.c **** PutChar('='); 2849 .LM487: 2850 0d3c 8DE3 ldi r24,lo8(61) 2851 0d3e 0E94 0000 call PutChar 926:srx1.c **** PutDecByte(ForwardCmd.status); 2853 .LM488: 2854 0d42 8091 0000 lds r24,ForwardCmd.1+7 2855 0d46 8DC1 rjmp .L220 2856 .L123: 927:srx1.c **** 928:srx1.c **** break; 929:srx1.c **** 930:srx1.c **** // Track/Follow Wall 931:srx1.c **** case ('T'): 932:srx1.c **** PutChar(':'); 2858 .LM489: 2859 0d48 8AE3 ldi r24,lo8(58) 2860 0d4a 0E94 0000 call PutChar 2861 .L124: 933:srx1.c **** do 934:srx1.c **** temp_char = GetChar(); 2863 .LM490: 2864 0d4e 0E94 0000 call GetChar 2865 0d52 082F mov r16,r24 2866 0d54 8C34 cpi r24,lo8(76) 2867 0d56 11F0 breq .L125 2868 0d58 8235 cpi r24,lo8(82) 2869 0d5a C9F7 brne .L124 2870 .L125: 935:srx1.c **** while (temp_char != 'L' && temp_char != 'R'); 936:srx1.c **** 937:srx1.c **** PutChar(temp_char); 2872 .LM491: 2873 0d5c 802F mov r24,r16 2874 0d5e 0E94 0000 call PutChar 938:srx1.c **** 939:srx1.c **** FollowCmd.ignore_door = LEFT; 2876 .LM492: 2877 0d62 1092 0000 sts FollowCmd.2+5,__zero_reg__ 940:srx1.c **** 941:srx1.c **** if (temp_char == 'L') 2879 .LM493: 2880 0d66 0C34 cpi r16,lo8(76) 2881 0d68 19F4 brne .L129 942:srx1.c **** FollowCmd.side = LEFT; 2883 .LM494: 2884 0d6a 1092 0000 sts FollowCmd.2+4,__zero_reg__ 2885 0d6e 03C0 rjmp .L130 2886 .L129: 943:srx1.c **** else 944:srx1.c **** FollowCmd.side = RIGHT; 2888 .LM495: 2889 0d70 81E0 ldi r24,lo8(1) 2890 0d72 8093 0000 sts FollowCmd.2+4,r24 2891 .L130: 945:srx1.c **** 946:srx1.c **** // Temporary disable EXEC's status 947:srx1.c **** ArbitrateCmd.info.active = FALSE; 2893 .LM496: 2894 0d76 1092 0000 sts ArbitrateCmd.3+6,__zero_reg__ 948:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2896 .LM497: 2897 0d7a 60E0 ldi r22,lo8(ArbitrateCmd.3) 2898 0d7c 70E0 ldi r23,hi8(ArbitrateCmd.3) 2899 0d7e 80E0 ldi r24,lo8(ArbitrateQueue) 2900 0d80 90E0 ldi r25,hi8(ArbitrateQueue) 2901 0d82 0E94 0000 call AvrXSendMessage 949:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2903 .LM498: 2904 0d86 80E0 ldi r24,lo8(ArbitrateCmd.3) 2905 0d88 90E0 ldi r25,hi8(ArbitrateCmd.3) 2906 0d8a 0E94 0000 call AvrXWaitMessageAck 950:srx1.c **** 951:srx1.c **** AvrXSendMessage(&FollowQueue, &FollowCmd.mcb); 2908 .LM499: 2909 0d8e 60E0 ldi r22,lo8(FollowCmd.2) 2910 0d90 70E0 ldi r23,hi8(FollowCmd.2) 2911 0d92 80E0 ldi r24,lo8(FollowQueue) 2912 0d94 90E0 ldi r25,hi8(FollowQueue) 2913 0d96 0E94 0000 call AvrXSendMessage 952:srx1.c **** AvrXWaitMessageAck(&FollowCmd.mcb); 2915 .LM500: 2916 0d9a 80E0 ldi r24,lo8(FollowCmd.2) 2917 0d9c 90E0 ldi r25,hi8(FollowCmd.2) 2918 0d9e 0E94 0000 call AvrXWaitMessageAck 953:srx1.c **** 954:srx1.c **** // restore our status 955:srx1.c **** ArbitrateCmd.info.active = TRUE; 2920 .LM501: 2921 0da2 81E0 ldi r24,lo8(1) 2922 0da4 8093 0000 sts ArbitrateCmd.3+6,r24 956:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2924 .LM502: 2925 0da8 60E0 ldi r22,lo8(ArbitrateCmd.3) 2926 0daa 70E0 ldi r23,hi8(ArbitrateCmd.3) 2927 0dac 80E0 ldi r24,lo8(ArbitrateQueue) 2928 0dae 90E0 ldi r25,hi8(ArbitrateQueue) 2929 0db0 0E94 0000 call AvrXSendMessage 957:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2931 .LM503: 2932 0db4 80E0 ldi r24,lo8(ArbitrateCmd.3) 2933 0db6 90E0 ldi r25,hi8(ArbitrateCmd.3) 2934 0db8 0E94 0000 call AvrXWaitMessageAck 958:srx1.c **** 959:srx1.c **** // Print FOLLOW's reported result 960:srx1.c **** PutChar('='); 2936 .LM504: 2937 0dbc 8DE3 ldi r24,lo8(61) 2938 0dbe 0E94 0000 call PutChar 961:srx1.c **** PutDecByte(FollowCmd.status); 2940 .LM505: 2941 0dc2 8091 0000 lds r24,FollowCmd.2+6 2942 0dc6 4DC1 rjmp .L220 2943 .L131: 962:srx1.c **** break; 963:srx1.c **** 964:srx1.c **** // Align with Wall/Door 965:srx1.c **** case ('n'): 966:srx1.c **** PutChar(':'); 2945 .LM506: 2946 0dc8 8AE3 ldi r24,lo8(58) 2947 0dca 0E94 0000 call PutChar 2948 .L132: 967:srx1.c **** do 968:srx1.c **** temp_char = GetChar(); 2950 .LM507: 2951 0dce 0E94 0000 call GetChar 2952 0dd2 082F mov r16,r24 2953 0dd4 8C34 cpi r24,lo8(76) 2954 0dd6 21F0 breq .L133 2955 0dd8 8235 cpi r24,lo8(82) 2956 0dda 11F0 breq .L133 2957 0ddc 8434 cpi r24,lo8(68) 2958 0dde B9F7 brne .L132 2959 .L133: 969:srx1.c **** while (temp_char != 'L' && temp_char != 'R' && temp_char != 'D'); 970:srx1.c **** 971:srx1.c **** PutChar(temp_char); 2961 .LM508: 2962 0de0 802F mov r24,r16 2963 0de2 0E94 0000 call PutChar 972:srx1.c **** 973:srx1.c **** if (temp_char == 'L') 2965 .LM509: 2966 0de6 0C34 cpi r16,lo8(76) 2967 0de8 19F4 brne .L137 974:srx1.c **** AlignCmd.type = ALIGN_LEFT; 2969 .LM510: 2970 0dea 1092 0000 sts AlignCmd.7+4,__zero_reg__ 2971 0dee 07C0 rjmp .L138 2972 .L137: 975:srx1.c **** else 976:srx1.c **** if (temp_char == 'D') 2974 .LM511: 2975 0df0 0434 cpi r16,lo8(68) 2976 0df2 11F4 brne .L139 977:srx1.c **** AlignCmd.type = ALIGN_DOOR; 2978 .LM512: 2979 0df4 82E0 ldi r24,lo8(2) 2980 0df6 01C0 rjmp .L217 2981 .L139: 978:srx1.c **** else 979:srx1.c **** AlignCmd.type = ALIGN_RIGHT; 2983 .LM513: 2984 0df8 81E0 ldi r24,lo8(1) 2985 .L217: 2986 0dfa 8093 0000 sts AlignCmd.7+4,r24 2987 .L138: 980:srx1.c **** 981:srx1.c **** // Temporary disable EXEC's status 982:srx1.c **** ArbitrateCmd.info.active = FALSE; 2989 .LM514: 2990 0dfe 1092 0000 sts ArbitrateCmd.3+6,__zero_reg__ 983:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2992 .LM515: 2993 0e02 60E0 ldi r22,lo8(ArbitrateCmd.3) 2994 0e04 70E0 ldi r23,hi8(ArbitrateCmd.3) 2995 0e06 80E0 ldi r24,lo8(ArbitrateQueue) 2996 0e08 90E0 ldi r25,hi8(ArbitrateQueue) 2997 0e0a 0E94 0000 call AvrXSendMessage 984:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2999 .LM516: 3000 0e0e 80E0 ldi r24,lo8(ArbitrateCmd.3) 3001 0e10 90E0 ldi r25,hi8(ArbitrateCmd.3) 3002 0e12 0E94 0000 call AvrXWaitMessageAck 985:srx1.c **** 986:srx1.c **** AvrXSendMessage(&AlignQueue, &AlignCmd.mcb); 3004 .LM517: 3005 0e16 60E0 ldi r22,lo8(AlignCmd.7) 3006 0e18 70E0 ldi r23,hi8(AlignCmd.7) 3007 0e1a 80E0 ldi r24,lo8(AlignQueue) 3008 0e1c 90E0 ldi r25,hi8(AlignQueue) 3009 0e1e 0E94 0000 call AvrXSendMessage 987:srx1.c **** AvrXWaitMessageAck(&AlignCmd.mcb); 3011 .LM518: 3012 0e22 80E0 ldi r24,lo8(AlignCmd.7) 3013 0e24 90E0 ldi r25,hi8(AlignCmd.7) 3014 0e26 0E94 0000 call AvrXWaitMessageAck 988:srx1.c **** 989:srx1.c **** // restore our status 990:srx1.c **** ArbitrateCmd.info.active = TRUE; 3016 .LM519: 3017 0e2a 81E0 ldi r24,lo8(1) 3018 0e2c 8093 0000 sts ArbitrateCmd.3+6,r24 991:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 3020 .LM520: 3021 0e30 60E0 ldi r22,lo8(ArbitrateCmd.3) 3022 0e32 70E0 ldi r23,hi8(ArbitrateCmd.3) 3023 0e34 80E0 ldi r24,lo8(ArbitrateQueue) 3024 0e36 90E0 ldi r25,hi8(ArbitrateQueue) 3025 0e38 0E94 0000 call AvrXSendMessage 992:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 3027 .LM521: 3028 0e3c 80E0 ldi r24,lo8(ArbitrateCmd.3) 3029 0e3e 90E0 ldi r25,hi8(ArbitrateCmd.3) 3030 0e40 0E94 0000 call AvrXWaitMessageAck 993:srx1.c **** 994:srx1.c **** // Print ALIGN's reported result 995:srx1.c **** PutChar('='); 3032 .LM522: 3033 0e44 8DE3 ldi r24,lo8(61) 3034 0e46 0E94 0000 call PutChar 996:srx1.c **** PutDecByte(AlignCmd.status); 3036 .LM523: 3037 0e4a 8091 0000 lds r24,AlignCmd.7+5 3038 0e4e 09C1 rjmp .L220 3039 .L141: 997:srx1.c **** break; 998:srx1.c **** 999:srx1.c **** // Go At X,Y location 1000:srx1.c **** case ('G'): 1001:srx1.c **** PutChar(' '); 3041 .LM524: 3042 0e50 80E2 ldi r24,lo8(32) 3043 0e52 0E94 0000 call PutChar 1002:srx1.c **** PutChar('X'); 3045 .LM525: 3046 0e56 88E5 ldi r24,lo8(88) 3047 0e58 0E94 0000 call PutChar 1003:srx1.c **** PutChar(':'); 3049 .LM526: 3050 0e5c 8AE3 ldi r24,lo8(58) 3051 0e5e 0E94 0000 call PutChar 1004:srx1.c **** GoAtCmd.x = GetDecSignedWord(); 3053 .LM527: 3054 0e62 0E94 0000 call GetDecSignedWord 3055 0e66 9093 0000 sts (GoAtCmd.4+4)+1,r25 3056 0e6a 8093 0000 sts GoAtCmd.4+4,r24 1005:srx1.c **** PutChar(' '); 3058 .LM528: 3059 0e6e 80E2 ldi r24,lo8(32) 3060 0e70 0E94 0000 call PutChar 1006:srx1.c **** PutChar('Y'); 3062 .LM529: 3063 0e74 89E5 ldi r24,lo8(89) 3064 0e76 0E94 0000 call PutChar 1007:srx1.c **** PutChar(':'); 3066 .LM530: 3067 0e7a 8AE3 ldi r24,lo8(58) 3068 0e7c 0E94 0000 call PutChar 1008:srx1.c **** GoAtCmd.y = GetDecSignedWord(); 3070 .LM531: 3071 0e80 0E94 0000 call GetDecSignedWord 3072 0e84 9093 0000 sts (GoAtCmd.4+6)+1,r25 3073 0e88 8093 0000 sts GoAtCmd.4+6,r24 1009:srx1.c **** PutChar(' '); 3075 .LM532: 3076 0e8c 80E2 ldi r24,lo8(32) 3077 0e8e 0E94 0000 call PutChar 1010:srx1.c **** PutChar('@'); 3079 .LM533: 3080 0e92 80E4 ldi r24,lo8(64) 3081 0e94 0E94 0000 call PutChar 1011:srx1.c **** PutChar(':'); 3083 .LM534: 3084 0e98 8AE3 ldi r24,lo8(58) 3085 0e9a 0E94 0000 call PutChar 1012:srx1.c **** GoAtCmd.theta = GetDecWord(); 3087 .LM535: 3088 0e9e 0E94 0000 call GetDecWord 3089 0ea2 9093 0000 sts (GoAtCmd.4+8)+1,r25 3090 0ea6 8093 0000 sts GoAtCmd.4+8,r24 1013:srx1.c **** 1014:srx1.c **** // Temporary disable EXEC's status 1015:srx1.c **** ArbitrateCmd.info.active = FALSE; 3092 .LM536: 3093 0eaa 1092 0000 sts ArbitrateCmd.3+6,__zero_reg__ 1016:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 3095 .LM537: 3096 0eae 60E0 ldi r22,lo8(ArbitrateCmd.3) 3097 0eb0 70E0 ldi r23,hi8(ArbitrateCmd.3) 3098 0eb2 80E0 ldi r24,lo8(ArbitrateQueue) 3099 0eb4 90E0 ldi r25,hi8(ArbitrateQueue) 3100 0eb6 0E94 0000 call AvrXSendMessage 1017:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 3102 .LM538: 3103 0eba 80E0 ldi r24,lo8(ArbitrateCmd.3) 3104 0ebc 90E0 ldi r25,hi8(ArbitrateCmd.3) 3105 0ebe 0E94 0000 call AvrXWaitMessageAck 1018:srx1.c **** 1019:srx1.c **** AvrXSendMessage(&GoAtQueue, &GoAtCmd.mcb); 3107 .LM539: 3108 0ec2 60E0 ldi r22,lo8(GoAtCmd.4) 3109 0ec4 70E0 ldi r23,hi8(GoAtCmd.4) 3110 0ec6 80E0 ldi r24,lo8(GoAtQueue) 3111 0ec8 90E0 ldi r25,hi8(GoAtQueue) 3112 0eca 0E94 0000 call AvrXSendMessage 1020:srx1.c **** AvrXWaitMessageAck(&GoAtCmd.mcb); 3114 .LM540: 3115 0ece 80E0 ldi r24,lo8(GoAtCmd.4) 3116 0ed0 90E0 ldi r25,hi8(GoAtCmd.4) 3117 .L225: 3118 0ed2 0E94 0000 call AvrXWaitMessageAck 1021:srx1.c **** 1022:srx1.c **** // restore our status 1023:srx1.c **** ArbitrateCmd.info.active = TRUE; 3120 .LM541: 3121 0ed6 FDC0 rjmp .L219 3122 .L142: 1024:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 1025:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 1026:srx1.c **** 1027:srx1.c **** break; 1028:srx1.c **** 1029:srx1.c **** // Search Room 1030:srx1.c **** case ('c'): 1031:srx1.c **** PutChar(':'); // Which side to scan on? 3124 .LM542: 3125 0ed8 8AE3 ldi r24,lo8(58) 3126 0eda 0E94 0000 call PutChar 3127 .L143: 1032:srx1.c **** do 1033:srx1.c **** temp_char = GetChar(); 3129 .LM543: 3130 0ede 0E94 0000 call GetChar 3131 0ee2 082F mov r16,r24 3132 0ee4 8C34 cpi r24,lo8(76) 3133 0ee6 11F0 breq .L144 3134 0ee8 8235 cpi r24,lo8(82) 3135 0eea C9F7 brne .L143 3136 .L144: 1034:srx1.c **** while (temp_char != 'L' && temp_char != 'R'); 1035:srx1.c **** 1036:srx1.c **** PutChar(temp_char); 3138 .LM544: 3139 0eec 802F mov r24,r16 3140 0eee 0E94 0000 call PutChar 1037:srx1.c **** 1038:srx1.c **** if (temp_char == 'L') 3142 .LM545: 3143 0ef2 0C34 cpi r16,lo8(76) 3144 0ef4 19F4 brne .L148 1039:srx1.c **** SearchCmd.side = LEFT; 3146 .LM546: 3147 0ef6 1092 0000 sts SearchCmd.6+5,__zero_reg__ 3148 0efa 03C0 rjmp .L149 3149 .L148: 1040:srx1.c **** else 1041:srx1.c **** SearchCmd.side = RIGHT; 3151 .LM547: 3152 0efc 81E0 ldi r24,lo8(1) 3153 0efe 8093 0000 sts SearchCmd.6+5,r24 3154 .L149: 1042:srx1.c **** 1043:srx1.c **** // Temporary disable EXEC's status 1044:srx1.c **** ArbitrateCmd.info.active = FALSE; 3156 .LM548: 3157 0f02 1092 0000 sts ArbitrateCmd.3+6,__zero_reg__ 1045:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 3159 .LM549: 3160 0f06 60E0 ldi r22,lo8(ArbitrateCmd.3) 3161 0f08 70E0 ldi r23,hi8(ArbitrateCmd.3) 3162 0f0a 80E0 ldi r24,lo8(ArbitrateQueue) 3163 0f0c 90E0 ldi r25,hi8(ArbitrateQueue) 3164 0f0e 0E94 0000 call AvrXSendMessage 1046:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 3166 .LM550: 3167 0f12 80E0 ldi r24,lo8(ArbitrateCmd.3) 3168 0f14 90E0 ldi r25,hi8(ArbitrateCmd.3) 3169 0f16 0E94 0000 call AvrXWaitMessageAck 1047:srx1.c **** 1048:srx1.c **** AvrXSendMessage(&SearchQueue, &SearchCmd.mcb); 3171 .LM551: 3172 0f1a 60E0 ldi r22,lo8(SearchCmd.6) 3173 0f1c 70E0 ldi r23,hi8(SearchCmd.6) 3174 0f1e 80E0 ldi r24,lo8(SearchQueue) 3175 0f20 90E0 ldi r25,hi8(SearchQueue) 3176 0f22 0E94 0000 call AvrXSendMessage 1049:srx1.c **** AvrXWaitMessageAck(&SearchCmd.mcb); 3178 .LM552: 3179 0f26 80E0 ldi r24,lo8(SearchCmd.6) 3180 0f28 90E0 ldi r25,hi8(SearchCmd.6) 3181 0f2a 0E94 0000 call AvrXWaitMessageAck 1050:srx1.c **** 1051:srx1.c **** // If candle was found, proceed to extinguish it. 1052:srx1.c **** if (SearchCmd.status == TRUE) 3183 .LM553: 3184 0f2e 8091 0000 lds r24,SearchCmd.6+4 3185 0f32 8130 cpi r24,lo8(1) 3186 0f34 51F4 brne .L150 1053:srx1.c **** { 1054:srx1.c **** AvrXSendMessage(&ExtinguishQueue, &ExtinguishCmd.mcb); 3188 .LM554: 3189 0f36 60E0 ldi r22,lo8(ExtinguishCmd.5) 3190 0f38 70E0 ldi r23,hi8(ExtinguishCmd.5) 3191 0f3a 80E0 ldi r24,lo8(ExtinguishQueue) 3192 0f3c 90E0 ldi r25,hi8(ExtinguishQueue) 3193 0f3e 0E94 0000 call AvrXSendMessage 1055:srx1.c **** AvrXWaitMessageAck(&ExtinguishCmd.mcb); 3195 .LM555: 3196 0f42 80E0 ldi r24,lo8(ExtinguishCmd.5) 3197 0f44 90E0 ldi r25,hi8(ExtinguishCmd.5) 3198 0f46 0E94 0000 call AvrXWaitMessageAck 3199 .L150: 1056:srx1.c **** } 1057:srx1.c **** 1058:srx1.c **** // Print result of search 1059:srx1.c **** PutChar('='); 3201 .LM556: 3202 0f4a 8DE3 ldi r24,lo8(61) 3203 0f4c 0E94 0000 call PutChar 1060:srx1.c **** if (SearchCmd.status == TRUE) 3205 .LM557: 3206 0f50 8091 0000 lds r24,SearchCmd.6+4 3207 0f54 8130 cpi r24,lo8(1) 3208 0f56 11F4 brne .L151 1061:srx1.c **** PutChar('1'); 3210 .LM558: 3211 0f58 81E3 ldi r24,lo8(49) 3212 0f5a 01C0 rjmp .L218 3213 .L151: 1062:srx1.c **** else 1063:srx1.c **** PutChar('0'); 3215 .LM559: 3216 0f5c 80E3 ldi r24,lo8(48) 3217 .L218: 3218 0f5e 0E94 0000 call PutChar 3219 0f62 B7C0 rjmp .L219 3220 .L153: 1064:srx1.c **** 1065:srx1.c **** // restore our status 1066:srx1.c **** ArbitrateCmd.info.active = TRUE; 1067:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 1068:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 1069:srx1.c **** break; 1070:srx1.c **** 1071:srx1.c **** // Fan debug 1072:srx1.c **** case (':'): 1073:srx1.c **** bit_toggle(FAN_CTL); 3222 .LM560: 3223 0f64 1A9B sbis 35-0x20,2 3224 0f66 02C0 rjmp .L154 3225 0f68 1A98 cbi 35-0x20,2 3226 0f6a C3C0 rjmp .L11 3227 .L154: 3228 0f6c 1A9A sbi 35-0x20,2 1074:srx1.c **** break; 3230 .LM561: 3231 0f6e C1C0 rjmp .L11 3232 .L158: 1075:srx1.c **** 1076:srx1.c **** // Toggle ARBITRATE debug output 1077:srx1.c **** case ('a'): 1078:srx1.c **** arbitrate_debug = arbitrate_debug ? FALSE : TRUE; 1079:srx1.c **** break; 1080:srx1.c **** 1081:srx1.c **** // Toggle FOLLOW debug output 1082:srx1.c **** case ('f'): 1083:srx1.c **** arbitrate_debug = arbitrate_debug ? FALSE : TRUE; 3234 .LM562: 3235 0f70 90E0 ldi r25,lo8(0) 3236 0f72 8091 0000 lds r24,arbitrate_debug 3237 0f76 8823 tst r24 3238 0f78 09F4 brne .L159 3239 0f7a 91E0 ldi r25,lo8(1) 3240 .L159: 3241 0f7c 9093 0000 sts arbitrate_debug,r25 1084:srx1.c **** break; 3243 .LM563: 3244 0f80 B8C0 rjmp .L11 3245 .L160: 1085:srx1.c **** 1086:srx1.c **** // Dump out SCM status & registers. 1087:srx1.c **** case ('g'): 1088:srx1.c **** for (temp_char = 0; temp_char < 14; temp_char++) 3247 .LM564: 3248 0f82 00E0 ldi r16,lo8(0) 3249 .L165: 1089:srx1.c **** { 1090:srx1.c **** PutCRLF(); 3251 .LM565: 3252 0f84 0E94 0000 call PutCRLF 1091:srx1.c **** PutDecByte(temp_char); 3254 .LM566: 3255 0f88 802F mov r24,r16 3256 0f8a 0E94 0000 call PutDecByte 1092:srx1.c **** PutChar(':'); 3258 .LM567: 3259 0f8e 8AE3 ldi r24,lo8(58) 3260 0f90 0E94 0000 call PutChar 1093:srx1.c **** PutHexByte(i2c_readbyte(SCM_I2C_ADDR, temp_char+SCM_CMD_COUNT)); 3262 .LM568: 3263 0f94 025F subi r16,lo8(-(14)) 3264 0f96 602F mov r22,r16 3265 0f98 80EB ldi r24,lo8(-80) 3266 0f9a 0E94 0000 call i2c_readbyte 3267 0f9e 0E94 0000 call PutHexByte 3269 .LM569: 3270 0fa2 0D50 subi r16,lo8(-(-13)) 3271 0fa4 0E30 cpi r16,lo8(14) 3272 0fa6 70F3 brlo .L165 1094:srx1.c **** } 1095:srx1.c **** PutCRLF(); 3274 .LM570: 3275 0fa8 0E94 0000 call PutCRLF 1096:srx1.c **** PutChar('A'); 3277 .LM571: 3278 0fac 81E4 ldi r24,lo8(65) 3279 0fae 0E94 0000 call PutChar 1097:srx1.c **** PutChar(':'); 3281 .LM572: 3282 0fb2 8AE3 ldi r24,lo8(58) 3283 0fb4 0E94 0000 call PutChar 1098:srx1.c **** PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_AVERAGE)); 3285 .LM573: 3286 0fb8 66E0 ldi r22,lo8(6) 3287 0fba 80EB ldi r24,lo8(-80) 3288 0fbc 0E94 0000 call i2c_readbyte 3289 0fc0 0E94 0000 call PutDecByte 1099:srx1.c **** PutCRLF(); 3291 .LM574: 3292 0fc4 0E94 0000 call PutCRLF 1100:srx1.c **** PutChar('T'); 3294 .LM575: 3295 0fc8 84E5 ldi r24,lo8(84) 3296 0fca 0E94 0000 call PutChar 1101:srx1.c **** PutChar(':'); 3298 .LM576: 3299 0fce 8AE3 ldi r24,lo8(58) 3300 0fd0 0E94 0000 call PutChar 1102:srx1.c **** PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_THRESHOLD)); 3302 .LM577: 3303 0fd4 67E0 ldi r22,lo8(7) 3304 0fd6 80EB ldi r24,lo8(-80) 3305 0fd8 0E94 0000 call i2c_readbyte 3306 0fdc 0E94 0000 call PutDecByte 1103:srx1.c **** PutCRLF(); 3308 .LM578: 3309 0fe0 0E94 0000 call PutCRLF 1104:srx1.c **** PutChar('C'); 3311 .LM579: 3312 0fe4 83E4 ldi r24,lo8(67) 3313 0fe6 0E94 0000 call PutChar 1105:srx1.c **** PutChar(':'); 3315 .LM580: 3316 0fea 8AE3 ldi r24,lo8(58) 3317 0fec 0E94 0000 call PutChar 1106:srx1.c **** PutDecWord((unsigned int)((i2c_readbyte(SCM_I2C_ADDR, SCM_COUNT_HI)<<8) | (i2c_readbyte(SCM_I2 3319 .LM581: 3320 0ff0 6BE0 ldi r22,lo8(11) 3321 0ff2 80EB ldi r24,lo8(-80) 3322 0ff4 0E94 0000 call i2c_readbyte 3323 0ff8 9927 clr r25 3324 0ffa 182F mov r17,r24 3325 0ffc 0027 clr r16 3326 0ffe 6AE0 ldi r22,lo8(10) 3327 1000 80EB ldi r24,lo8(-80) 3328 1002 0E94 0000 call i2c_readbyte 3329 1006 9927 clr r25 3330 1008 082B or r16,r24 3331 100a 192B or r17,r25 3332 100c C801 movw r24,r16 3333 100e 0E94 0000 call PutDecWord 1107:srx1.c **** PutCRLF(); 3335 .LM582: 3336 1012 0E94 0000 call PutCRLF 1108:srx1.c **** PutChar('H'); 3338 .LM583: 3339 1016 88E4 ldi r24,lo8(72) 3340 1018 0E94 0000 call PutChar 1109:srx1.c **** PutChar(':'); 3342 .LM584: 3343 101c 8AE3 ldi r24,lo8(58) 3344 101e 0E94 0000 call PutChar 1110:srx1.c **** PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_PIXEL_MAX)); 3346 .LM585: 3347 1022 68E0 ldi r22,lo8(8) 3348 1024 80EB ldi r24,lo8(-80) 3349 1026 0E94 0000 call i2c_readbyte 3350 102a 0E94 0000 call PutDecByte 1111:srx1.c **** PutCRLF(); 3352 .LM586: 3353 102e 0E94 0000 call PutCRLF 1112:srx1.c **** PutChar('L'); 3355 .LM587: 3356 1032 8CE4 ldi r24,lo8(76) 3357 1034 0E94 0000 call PutChar 1113:srx1.c **** PutChar(':'); 3359 .LM588: 3360 1038 8AE3 ldi r24,lo8(58) 3361 103a 0E94 0000 call PutChar 1114:srx1.c **** PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_PIXEL_MIN)); 3363 .LM589: 3364 103e 69E0 ldi r22,lo8(9) 3365 1040 80EB ldi r24,lo8(-80) 3366 1042 0E94 0000 call i2c_readbyte 3367 1046 0E94 0000 call PutDecByte 1115:srx1.c **** PutCRLF(); 3369 .LM590: 3370 104a 0E94 0000 call PutCRLF 1116:srx1.c **** PutChar('S'); 3372 .LM591: 3373 104e 83E5 ldi r24,lo8(83) 3374 1050 0E94 0000 call PutChar 1117:srx1.c **** PutChar(':'); 3376 .LM592: 3377 1054 8AE3 ldi r24,lo8(58) 3378 1056 0E94 0000 call PutChar 1118:srx1.c **** PutDecByte(i2c_readbyte(SCM_I2C_ADDR, SCM_TARGET_MIN_SIZE)); 3380 .LM593: 3381 105a 6CE0 ldi r22,lo8(12) 3382 105c 80EB ldi r24,lo8(-80) 3383 105e 0E94 0000 call i2c_readbyte 3384 .L220: 3385 1062 0E94 0000 call PutDecByte 1119:srx1.c **** break; 3387 .LM594: 3388 1066 45C0 rjmp .L11 3389 .L166: 1120:srx1.c **** 1121:srx1.c **** // Reinitialize Parameter table in EEPROM (DEFAULTS->EEPROM). 1122:srx1.c **** case ('I'): 1123:srx1.c **** InitParameterTable(); 3391 .LM595: 3392 1068 0E94 0000 call InitParameterTable 1124:srx1.c **** break; 3394 .LM596: 3395 106c 42C0 rjmp .L11 3396 .L167: 1125:srx1.c **** 1126:srx1.c **** // Reload Parameter table (EEPROM->SRAM). 1127:srx1.c **** case ('L'): 1128:srx1.c **** LoadParameterTable(); 3398 .LM597: 3399 106e 0E94 0000 call LoadParameterTable 1129:srx1.c **** break; 3401 .LM598: 3402 1072 3FC0 rjmp .L11 3403 .L168: 1130:srx1.c **** 1131:srx1.c **** // Dump Parameter Table 1132:srx1.c **** case ('P'): 1133:srx1.c **** DumpParameterTable(); 3405 .LM599: 3406 1074 0E94 0000 call DumpParameterTable 1134:srx1.c **** break; 3408 .LM600: 3409 1078 3CC0 rjmp .L11 3410 .L169: 1135:srx1.c **** 1136:srx1.c **** // Edit Parameter Table 1137:srx1.c **** case ('E'): 1138:srx1.c **** PrintString(str_parameter_edit); 3412 .LM601: 3413 107a 80E0 ldi r24,lo8(str_parameter_edit) 3414 107c 90E0 ldi r25,hi8(str_parameter_edit) 3415 107e 0E94 0000 call PrintString 1139:srx1.c **** temp_char = GetDecWord(); 3417 .LM602: 3418 1082 0E94 0000 call GetDecWord 1140:srx1.c **** EditParameterTable(temp_char); 3420 .LM603: 3421 1086 0E94 0000 call EditParameterTable 1141:srx1.c **** break; 3423 .LM604: 3424 108a 33C0 rjmp .L11 3425 .L170: 1142:srx1.c **** 1143:srx1.c **** // kill PLANNER for this session 1144:srx1.c **** case ('k'): 1145:srx1.c **** AvrXSuspend(PID(planner)); 3427 .LM605: 3428 108c 80E0 ldi r24,lo8(plannerPid) 3429 108e 90E0 ldi r25,hi8(plannerPid) 3430 1090 0E94 0000 call AvrXSuspend 1146:srx1.c **** break; 3432 .LM606: 3433 1094 2EC0 rjmp .L11 3434 .L171: 1147:srx1.c **** 1148:srx1.c **** // Resurrect PLANNER 1149:srx1.c **** case ('l'): 1150:srx1.c **** AvrXRunTask(TCB(planner)); 3436 .LM607: 3437 1096 80E0 ldi r24,lo8(plannerTcb) 3438 1098 90E0 ldi r25,hi8(plannerTcb) 3439 109a 0E94 0000 call AvrXRunTask 1151:srx1.c **** break; 3441 .LM608: 3442 109e 29C0 rjmp .L11 3443 .L172: 1152:srx1.c **** 1153:srx1.c **** // Request SCM auto-calibration 1154:srx1.c **** case ('x'): 1155:srx1.c **** SCM_calibrate(TRUE); 3445 .LM609: 3446 10a0 81E0 ldi r24,lo8(1) 3447 10a2 0E94 0000 call SCM_calibrate 1156:srx1.c **** break; 3449 .LM610: 3450 10a6 25C0 rjmp .L11 3451 .L173: 1157:srx1.c **** 1158:srx1.c **** // Go Home! 1159:srx1.c **** case ('h'): 1160:srx1.c **** // Temporary disable EXEC's status 1161:srx1.c **** PutChar(':'); 3453 .LM611: 3454 10a8 8AE3 ldi r24,lo8(58) 3455 10aa 0E94 0000 call PutChar 1162:srx1.c **** temp_char = GetDecWord(); 3457 .LM612: 3458 10ae 0E94 0000 call GetDecWord 1163:srx1.c **** Room = temp_char; 3460 .LM613: 3461 10b2 8093 0000 sts Room,r24 1164:srx1.c **** ArbitrateCmd.info.active = FALSE; 3463 .LM614: 3464 10b6 1092 0000 sts ArbitrateCmd.3+6,__zero_reg__ 1165:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 3466 .LM615: 3467 10ba 60E0 ldi r22,lo8(ArbitrateCmd.3) 3468 10bc 70E0 ldi r23,hi8(ArbitrateCmd.3) 3469 10be 80E0 ldi r24,lo8(ArbitrateQueue) 3470 10c0 90E0 ldi r25,hi8(ArbitrateQueue) 3471 10c2 0E94 0000 call AvrXSendMessage 1166:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 3473 .LM616: 3474 10c6 80E0 ldi r24,lo8(ArbitrateCmd.3) 3475 10c8 90E0 ldi r25,hi8(ArbitrateCmd.3) 3476 10ca 0E94 0000 call AvrXWaitMessageAck 1167:srx1.c **** 1168:srx1.c **** GoHome(); 3478 .LM617: 3479 10ce 0E94 0000 call GoHome 3480 .L219: 1169:srx1.c **** 1170:srx1.c **** // restore our status 1171:srx1.c **** ArbitrateCmd.info.active = TRUE; 3482 .LM618: 3483 10d2 81E0 ldi r24,lo8(1) 3484 10d4 8093 0000 sts ArbitrateCmd.3+6,r24 3485 .L222: 1172:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 3487 .LM619: 3488 10d8 60E0 ldi r22,lo8(ArbitrateCmd.3) 3489 10da 70E0 ldi r23,hi8(ArbitrateCmd.3) 3490 10dc 80E0 ldi r24,lo8(ArbitrateQueue) 3491 10de 90E0 ldi r25,hi8(ArbitrateQueue) 3492 10e0 0E94 0000 call AvrXSendMessage 1173:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 3494 .LM620: 3495 10e4 80E0 ldi r24,lo8(ArbitrateCmd.3) 3496 10e6 90E0 ldi r25,hi8(ArbitrateCmd.3) 3497 10e8 0E94 0000 call AvrXWaitMessageAck 1174:srx1.c **** break; 3499 .LM621: 3500 10ec 02C0 rjmp .L11 3501 .L174: 1175:srx1.c **** 1176:srx1.c **** // Reset Odometry 1177:srx1.c **** case ('o'): 1178:srx1.c **** ResetOdometry(); 3503 .LM622: 3504 10ee 0E94 0000 call ResetOdometry 1179:srx1.c **** break; 1180:srx1.c **** 1181:srx1.c **** default: 1182:srx1.c **** break; 3506 .LM623: 3507 .L11: 3508 10f2 FF20 tst r15 3509 10f4 11F4 brne .+4 3510 10f6 0C94 0000 jmp .L177 1183:srx1.c **** } 1184:srx1.c **** } 1185:srx1.c **** 1186:srx1.c **** // Enable printing of velocity by MotorTask 1187:srx1.c **** velocity_print = TRUE; 3512 .LM624: 3513 10fa 81E0 ldi r24,lo8(1) 3514 10fc 8093 0000 sts velocity_print,r24 1188:srx1.c **** 1189:srx1.c **** exit = FALSE; 3516 .LM625: 3517 1100 FF24 clr r15 1190:srx1.c **** 1191:srx1.c **** // We've quit, release sumbsumption flag 1192:srx1.c **** //ArbitrateCmd.info.output = STOP; 1193:srx1.c **** ArbitrateCmd.info.active = FALSE; 3519 .LM626: 3520 1102 F092 0000 sts ArbitrateCmd.3+6,r15 1194:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 3522 .LM627: 3523 1106 60E0 ldi r22,lo8(ArbitrateCmd.3) 3524 1108 70E0 ldi r23,hi8(ArbitrateCmd.3) 3525 110a 80E0 ldi r24,lo8(ArbitrateQueue) 3526 110c 90E0 ldi r25,hi8(ArbitrateQueue) 3527 110e 0E94 0000 call AvrXSendMessage 1195:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 3529 .LM628: 3530 1112 80E0 ldi r24,lo8(ArbitrateCmd.3) 3531 1114 90E0 ldi r25,hi8(ArbitrateCmd.3) 3532 1116 0E94 0000 call AvrXWaitMessageAck 3533 111a 0C94 0000 jmp .L183 1196:srx1.c **** 1197:srx1.c **** // Resume halted tasks 1198:srx1.c **** //AvrXResume(PID(MotorTask)); 1199:srx1.c **** //AvrXResume(PID(follow)); 1200:srx1.c **** } 1201:srx1.c **** } 3535 .LM629: 3536 .LBE2: 3537 /* epilogue: frame size=0 */ 3538 /* epilogue: naked */ 3539 /* epilogue end (size=0) */ 3540 /* function exec size 2267 (2267) */ 3557 .Lscope1: 3560 .global planner 3562 planner: 1202:srx1.c **** 1203:srx1.c **** /****************************************** 1204:srx1.c **** Task - PLANNER 1205:srx1.c **** ******************************************/ 1206:srx1.c **** NAKEDFUNC(planner) 1207:srx1.c **** { 3564 .LM630: 3565 /* prologue: frame size=0 */ 3566 /* prologue: naked */ 3567 /* prologue end (size=0) */ 1208:srx1.c **** // Check for SCM calibration request via STK300's PORTD switches. (bit 7) 1209:srx1.c **** if (bit_test(SCM_CALIBRATE_REQ) == FALSE) 3569 .LM631: 3570 111e 8799 sbic 48-0x20,7 3571 1120 03C0 rjmp .L229 1210:srx1.c **** { 1211:srx1.c **** SCM_calibrate(TRUE); 3573 .LM632: 3574 1122 81E0 ldi r24,lo8(1) 3575 1124 0E94 0000 call SCM_calibrate 3576 .L229: 1212:srx1.c **** } 1213:srx1.c **** 1214:srx1.c **** // Check for Line Sensor calibration request via STK300's PORTD switches. (bit 6) 1215:srx1.c **** if (bit_test(LINE_CALIBRATE_REQ) == FALSE) 3578 .LM633: 3579 1128 8699 sbic 48-0x20,6 3580 112a 03C0 rjmp .L230 1216:srx1.c **** { 1217:srx1.c **** Line_calibrate(TRUE); 3582 .LM634: 3583 112c 81E0 ldi r24,lo8(1) 3584 112e 0E94 0000 call Line_calibrate 3585 .L230: 1218:srx1.c **** } 1219:srx1.c **** 1220:srx1.c **** // Align with Left Wall before waiting for Start signal 1221:srx1.c **** //Align(ALIGN_LEFT); 1222:srx1.c **** 1223:srx1.c **** // Initial delay to give a chance to Motion sensor to stabilize 1224:srx1.c **** AvrXDelay(&planner_timer, STARTUP_DELAY); 3587 .LM635: 3588 1132 68E9 ldi r22,lo8(15000) 3589 1134 7AE3 ldi r23,hi8(15000) 3590 1136 80E0 ldi r24,lo8(planner_timer) 3591 1138 90E0 ldi r25,hi8(planner_timer) 3592 113a 0E94 0000 call AvrXDelay 1225:srx1.c **** AvrXDelay(&planner_timer, STARTUP_DELAY); 3594 .LM636: 3595 113e 68E9 ldi r22,lo8(15000) 3596 1140 7AE3 ldi r23,hi8(15000) 3597 1142 80E0 ldi r24,lo8(planner_timer) 3598 1144 90E0 ldi r25,hi8(planner_timer) 3599 1146 0E94 0000 call AvrXDelay 1226:srx1.c **** AvrXDelay(&planner_timer, STARTUP_DELAY); 3601 .LM637: 3602 114a 68E9 ldi r22,lo8(15000) 3603 114c 7AE3 ldi r23,hi8(15000) 3604 114e 80E0 ldi r24,lo8(planner_timer) 3605 1150 90E0 ldi r25,hi8(planner_timer) 3606 1152 0E94 0000 call AvrXDelay 1227:srx1.c **** 1228:srx1.c **** // (this is where the sound detector board loop would go) 1229:srx1.c **** 1230:srx1.c **** // Reset Room number var. 1231:srx1.c **** Room = 0; 3608 .LM638: 3609 1156 1092 0000 sts Room,__zero_reg__ 1232:srx1.c **** 1233:srx1.c **** // 1234:srx1.c **** // HOME -> ROOM #1 1235:srx1.c **** // 1236:srx1.c **** 1237:srx1.c **** // Move forward for fixed amount to clear the home circle 1238:srx1.c **** Forward(FORWARD_CLEAR_HOME_CIRCLE, FORWARD_REPORT_SUCCESS); 3611 .LM639: 3612 115a 61E0 ldi r22,lo8(1) 3613 115c 88E0 ldi r24,lo8(8) 3614 115e 0E94 0000 call Forward 1239:srx1.c **** 1240:srx1.c **** // follow the left wall until wall ends. 1241:srx1.c **** Follow(LEFT, FOLLOW_REPORT_SUCCESS); 3616 .LM640: 3617 1162 61E0 ldi r22,lo8(1) 3618 1164 80E0 ldi r24,lo8(0) 3619 1166 0E94 0000 call Follow 1242:srx1.c **** 1243:srx1.c **** // Go forward 'till obstacle in front at threshold 1244:srx1.c **** Forward(0, FORWARD_REPORT_OBSTACLE); 3621 .LM641: 3622 116a 62E0 ldi r22,lo8(2) 3623 116c 80E0 ldi r24,lo8(0) 3624 116e 0E94 0000 call Forward 1245:srx1.c **** 1246:srx1.c **** // Rotate Left 90 degrees 1247:srx1.c **** Rotate(-90, FALSE); 3626 .LM642: 3627 1172 60E0 ldi r22,lo8(0) 3628 1174 86EA ldi r24,lo8(-90) 3629 1176 9FEF ldi r25,hi8(-90) 3630 1178 0E94 0000 call Rotate 1248:srx1.c **** 1249:srx1.c **** // Align with Right Wall 1250:srx1.c **** Align(ALIGN_RIGHT); 3632 .LM643: 3633 117c 81E0 ldi r24,lo8(1) 3634 117e 0E94 0000 call Align 1251:srx1.c **** 1252:srx1.c **** // Move forward for fixed amount 1253:srx1.c **** Forward(46, FORWARD_REPORT_SUCCESS); // TODO: Replace with a more robust algorithm. (no hard-cod 3636 .LM644: 3637 1182 61E0 ldi r22,lo8(1) 3638 1184 8EE2 ldi r24,lo8(46) 3639 1186 0E94 0000 call Forward 1254:srx1.c **** 1255:srx1.c **** // Align with Right Wall 1256:srx1.c **** Align(ALIGN_RIGHT); 3641 .LM645: 3642 118a 81E0 ldi r24,lo8(1) 3643 118c 0E94 0000 call Align 1257:srx1.c **** 1258:srx1.c **** // Rotate Left 90 degrees 1259:srx1.c **** Rotate(-90, FALSE); 3645 .LM646: 3646 1190 60E0 ldi r22,lo8(0) 3647 1192 86EA ldi r24,lo8(-90) 3648 1194 9FEF ldi r25,hi8(-90) 3649 1196 0E94 0000 call Rotate 1260:srx1.c **** 1261:srx1.c **** // Go forward 'till door detected 1262:srx1.c **** Forward(0, FORWARD_REPORT_DOOR); 3651 .LM647: 3652 119a 63E0 ldi r22,lo8(3) 3653 119c 80E0 ldi r24,lo8(0) 3654 119e 0E94 0000 call Forward 1263:srx1.c **** 1264:srx1.c **** // Align with Door frame 1265:srx1.c **** //Align(ALIGN_DOOR); 1266:srx1.c **** 1267:srx1.c **** // We're in room #1 1268:srx1.c **** Room = 1; 3656 .LM648: 3657 11a2 C1E0 ldi r28,lo8(1) 3658 11a4 C093 0000 sts Room,r28 1269:srx1.c **** 1270:srx1.c **** // Configure SEARCH to search current room. 1271:srx1.c **** if (Search(RIGHT) == TRUE) 3660 .LM649: 3661 11a8 8C2F mov r24,r28 3662 11aa 0E94 0000 call Search 3663 11ae 8C17 cp r24,r28 3664 11b0 11F4 brne .L231 1272:srx1.c **** GoHome(); 3666 .LM650: 3667 11b2 0E94 0000 call GoHome 3668 .L231: 1273:srx1.c **** 1274:srx1.c **** // 1275:srx1.c **** // ROOM #1 -> ROOM #2 1276:srx1.c **** // 1277:srx1.c **** 1278:srx1.c **** // I guess the candle was not found so align with Right Wall 1279:srx1.c **** Align(ALIGN_RIGHT); 3670 .LM651: 3671 11b6 8C2F mov r24,r28 3672 11b8 0E94 0000 call Align 1280:srx1.c **** 1281:srx1.c **** // And go forward until door is found. 1282:srx1.c **** Forward(0, FORWARD_REPORT_DOOR); 3674 .LM652: 3675 11bc 63E0 ldi r22,lo8(3) 3676 11be 80E0 ldi r24,lo8(0) 3677 11c0 0E94 0000 call Forward 1283:srx1.c **** 1284:srx1.c **** // Align with Door frame (and clear it) 1285:srx1.c **** //Align(ALIGN_DOOR); 1286:srx1.c **** 1287:srx1.c **** // Move forward to clear door frame 1288:srx1.c **** //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); 1289:srx1.c **** 1290:srx1.c **** // Move forward 'till wall 1291:srx1.c **** Forward(0, FORWARD_REPORT_OBSTACLE); 3679 .LM653: 3680 11c4 62E0 ldi r22,lo8(2) 3681 11c6 80E0 ldi r24,lo8(0) 3682 11c8 0E94 0000 call Forward 1292:srx1.c **** 1293:srx1.c **** // Rotate Left 90 degrees 1294:srx1.c **** Rotate(-90, FALSE); 3684 .LM654: 3685 11cc 60E0 ldi r22,lo8(0) 3686 11ce 86EA ldi r24,lo8(-90) 3687 11d0 9FEF ldi r25,hi8(-90) 3688 11d2 0E94 0000 call Rotate 1295:srx1.c **** 1296:srx1.c **** // Align with Right Wall 1297:srx1.c **** Align(ALIGN_RIGHT); 3690 .LM655: 3691 11d6 8C2F mov r24,r28 3692 11d8 0E94 0000 call Align 1298:srx1.c **** 1299:srx1.c **** // Go forward 'till wall detected. 1300:srx1.c **** Forward(0, FORWARD_REPORT_OBSTACLE); 3694 .LM656: 3695 11dc 62E0 ldi r22,lo8(2) 3696 11de 80E0 ldi r24,lo8(0) 3697 11e0 0E94 0000 call Forward 1301:srx1.c **** 1302:srx1.c **** // Align with Left Wall 1303:srx1.c **** Align(ALIGN_LEFT); 3699 .LM657: 3700 11e4 80E0 ldi r24,lo8(0) 3701 11e6 0E94 0000 call Align 1304:srx1.c **** 1305:srx1.c **** // Rotate Right 90 degrees 1306:srx1.c **** Rotate(+90, FALSE); 3703 .LM658: 3704 11ea 60E0 ldi r22,lo8(0) 3705 11ec 8AE5 ldi r24,lo8(90) 3706 11ee 90E0 ldi r25,hi8(90) 3707 11f0 0E94 0000 call Rotate 1307:srx1.c **** 1308:srx1.c **** // follow the left wall until door frame. 1309:srx1.c **** Follow(LEFT, FOLLOW_REPORT_DOOR); 3709 .LM659: 3710 11f4 63E0 ldi r22,lo8(3) 3711 11f6 80E0 ldi r24,lo8(0) 3712 11f8 0E94 0000 call Follow 1310:srx1.c **** 1311:srx1.c **** // Align with Door frame 1312:srx1.c **** //Align(ALIGN_DOOR); 1313:srx1.c **** 1314:srx1.c **** // We're in room #2 1315:srx1.c **** Room = 2; 3714 .LM660: 3715 11fc 12E0 ldi r17,lo8(2) 3716 11fe 1093 0000 sts Room,r17 1316:srx1.c **** 1317:srx1.c **** // Configure SEARCH to search current room. 1318:srx1.c **** if (Search(RIGHT) == TRUE) 3718 .LM661: 3719 1202 8C2F mov r24,r28 3720 1204 0E94 0000 call Search 3721 1208 8130 cpi r24,lo8(1) 3722 120a 11F4 brne .L232 1319:srx1.c **** GoHome(); 3724 .LM662: 3725 120c 0E94 0000 call GoHome 3726 .L232: 1320:srx1.c **** 1321:srx1.c **** // 1322:srx1.c **** // ROOM #2 -> ROOM #3 1323:srx1.c **** // 1324:srx1.c **** 1325:srx1.c **** // I guess the candle was not found so exit room by following Right wall. 1326:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_DOOR); 3728 .LM663: 3729 1210 63E0 ldi r22,lo8(3) 3730 1212 8C2F mov r24,r28 3731 1214 0E94 0000 call Follow 1327:srx1.c **** 1328:srx1.c **** // Align with Door frame (and clear it) 1329:srx1.c **** //Align(ALIGN_DOOR); 1330:srx1.c **** 1331:srx1.c **** // Move forward to clear door frame 1332:srx1.c **** //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); 1333:srx1.c **** 1334:srx1.c **** // Keep following right wall 'till obstacle 1335:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_OBSTACLE); 3733 .LM664: 3734 1218 612F mov r22,r17 3735 121a 8C2F mov r24,r28 3736 121c 0E94 0000 call Follow 1336:srx1.c **** 1337:srx1.c **** // Rotate Left 90 degrees 1338:srx1.c **** Rotate(-90, FALSE); 3738 .LM665: 3739 1220 60E0 ldi r22,lo8(0) 3740 1222 86EA ldi r24,lo8(-90) 3741 1224 9FEF ldi r25,hi8(-90) 3742 1226 0E94 0000 call Rotate 1339:srx1.c **** 1340:srx1.c **** // Align with Right wall 1341:srx1.c **** Align(ALIGN_RIGHT); 3744 .LM666: 3745 122a 8C2F mov r24,r28 3746 122c 0E94 0000 call Align 1342:srx1.c **** 1343:srx1.c **** // Go forward a fixed amount so we can find left wall 1344:srx1.c **** Forward(51, FORWARD_REPORT_SUCCESS); 3748 .LM667: 3749 1230 6C2F mov r22,r28 3750 1232 83E3 ldi r24,lo8(51) 3751 1234 0E94 0000 call Forward 1345:srx1.c **** 1346:srx1.c **** // TODO: Add a call to ALIGN to add robustness and give a break to FOLLOW??? 1347:srx1.c **** 1348:srx1.c **** // Left wall should now be in range. Follow it. 1349:srx1.c **** Follow(LEFT, FOLLOW_REPORT_SUCCESS); 3753 .LM668: 3754 1238 6C2F mov r22,r28 3755 123a 80E0 ldi r24,lo8(0) 3756 123c 0E94 0000 call Follow 1350:srx1.c **** 1351:srx1.c **** // Go forward a fixed amount so we can find Right wall 1352:srx1.c **** Forward(24, FORWARD_REPORT_SUCCESS); 3758 .LM669: 3759 1240 6C2F mov r22,r28 3760 1242 88E1 ldi r24,lo8(24) 3761 1244 0E94 0000 call Forward 1353:srx1.c **** 1354:srx1.c **** // Align with Right Wall 1355:srx1.c **** Align(ALIGN_RIGHT); 3763 .LM670: 3764 1248 8C2F mov r24,r28 3765 124a 0E94 0000 call Align 1356:srx1.c **** 1357:srx1.c **** // Rotate Left 90 degrees 1358:srx1.c **** Rotate(-90, FALSE); 3767 .LM671: 3768 124e 60E0 ldi r22,lo8(0) 3769 1250 86EA ldi r24,lo8(-90) 3770 1252 9FEF ldi r25,hi8(-90) 3771 1254 0E94 0000 call Rotate 1359:srx1.c **** 1360:srx1.c **** // Move forward for fixed amount 1361:srx1.c **** Forward(49, FORWARD_REPORT_SUCCESS); 3773 .LM672: 3774 1258 6C2F mov r22,r28 3775 125a 81E3 ldi r24,lo8(49) 3776 125c 0E94 0000 call Forward 1362:srx1.c **** 1363:srx1.c **** // Left wall should now be in range. Follow it. 1364:srx1.c **** Follow(LEFT, FOLLOW_REPORT_OBSTACLE); 3778 .LM673: 3779 1260 612F mov r22,r17 3780 1262 80E0 ldi r24,lo8(0) 3781 1264 0E94 0000 call Follow 1365:srx1.c **** 1366:srx1.c **** // Rotate Right 90 degrees 1367:srx1.c **** Rotate(+90, FALSE); 3783 .LM674: 3784 1268 60E0 ldi r22,lo8(0) 3785 126a 8AE5 ldi r24,lo8(90) 3786 126c 90E0 ldi r25,hi8(90) 3787 126e 0E94 0000 call Rotate 1368:srx1.c **** 1369:srx1.c **** // Align with Left Wall 1370:srx1.c **** Align(ALIGN_LEFT); 3789 .LM675: 3790 1272 80E0 ldi r24,lo8(0) 3791 1274 0E94 0000 call Align 1371:srx1.c **** 1372:srx1.c **** // Follow Left wall to door 1373:srx1.c **** Follow(LEFT, FOLLOW_REPORT_DOOR); 3793 .LM676: 3794 1278 63E0 ldi r22,lo8(3) 3795 127a 80E0 ldi r24,lo8(0) 3796 127c 0E94 0000 call Follow 1374:srx1.c **** 1375:srx1.c **** // Align with door frame 1376:srx1.c **** //Align(ALIGN_DOOR); 1377:srx1.c **** 1378:srx1.c **** // We're in room #3 1379:srx1.c **** Room = 3; 3798 .LM677: 3799 1280 03E0 ldi r16,lo8(3) 3800 1282 0093 0000 sts Room,r16 1380:srx1.c **** 1381:srx1.c **** // Configure SEARCH to search current room. 1382:srx1.c **** if (Search(RIGHT) == TRUE) 3802 .LM678: 3803 1286 8C2F mov r24,r28 3804 1288 0E94 0000 call Search 3805 128c 8130 cpi r24,lo8(1) 3806 128e 11F4 brne .L233 1383:srx1.c **** GoHome(); 3808 .LM679: 3809 1290 0E94 0000 call GoHome 3810 .L233: 1384:srx1.c **** 1385:srx1.c **** // 1386:srx1.c **** // ROOM #3 -> ROOM #4 1387:srx1.c **** // 1388:srx1.c **** 1389:srx1.c **** // I guess the candle was not found so exit room by following Right wall. 1390:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_DOOR); 3812 .LM680: 3813 1294 602F mov r22,r16 3814 1296 8C2F mov r24,r28 3815 1298 0E94 0000 call Follow 1391:srx1.c **** 1392:srx1.c **** // Align with door frame (and clear it) 1393:srx1.c **** //Align(ALIGN_DOOR); 1394:srx1.c **** 1395:srx1.c **** // Move forward to clear door frame 1396:srx1.c **** //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); 1397:srx1.c **** 1398:srx1.c **** // Keep following right wall 'till obstacle 1399:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_OBSTACLE); 3817 .LM681: 3818 129c 612F mov r22,r17 3819 129e 8C2F mov r24,r28 3820 12a0 0E94 0000 call Follow 1400:srx1.c **** 1401:srx1.c **** // Rotate Left 90 degrees 1402:srx1.c **** Rotate(-90, FALSE); 3822 .LM682: 3823 12a4 60E0 ldi r22,lo8(0) 3824 12a6 86EA ldi r24,lo8(-90) 3825 12a8 9FEF ldi r25,hi8(-90) 3826 12aa 0E94 0000 call Rotate 1403:srx1.c **** 1404:srx1.c **** // Align with Right wall 1405:srx1.c **** Align(ALIGN_RIGHT); 3828 .LM683: 3829 12ae 8C2F mov r24,r28 3830 12b0 0E94 0000 call Align 1406:srx1.c **** 1407:srx1.c **** // Follow right wall 'till the end 1408:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_SUCCESS); 3832 .LM684: 3833 12b4 6C2F mov r22,r28 3834 12b6 8C2F mov r24,r28 3835 12b8 0E94 0000 call Follow 1409:srx1.c **** 1410:srx1.c **** // Move forward 'till obstacle detected 1411:srx1.c **** Forward(0, FORWARD_REPORT_OBSTACLE); 3837 .LM685: 3838 12bc 612F mov r22,r17 3839 12be 80E0 ldi r24,lo8(0) 3840 12c0 0E94 0000 call Forward 1412:srx1.c **** 1413:srx1.c **** // Rotate Left 90 degrees 1414:srx1.c **** Rotate(-90, FALSE); 3842 .LM686: 3843 12c4 60E0 ldi r22,lo8(0) 3844 12c6 86EA ldi r24,lo8(-90) 3845 12c8 9FEF ldi r25,hi8(-90) 3846 12ca 0E94 0000 call Rotate 1415:srx1.c **** 1416:srx1.c **** // Align with Right wall 1417:srx1.c **** Align(ALIGN_RIGHT); 3848 .LM687: 3849 12ce 8C2F mov r24,r28 3850 12d0 0E94 0000 call Align 1418:srx1.c **** 1419:srx1.c **** // Follow Right wall 'till obstacle detected. 1420:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_SUCCESS); 3852 .LM688: 3853 12d4 6C2F mov r22,r28 3854 12d6 8C2F mov r24,r28 3855 12d8 0E94 0000 call Follow 1421:srx1.c **** 1422:srx1.c **** // Move forward till obstacle 1423:srx1.c **** Forward(0, FORWARD_REPORT_OBSTACLE); 3857 .LM689: 3858 12dc 612F mov r22,r17 3859 12de 80E0 ldi r24,lo8(0) 3860 12e0 0E94 0000 call Forward 1424:srx1.c **** 1425:srx1.c **** // Align with Left wall 1426:srx1.c **** Align(ALIGN_LEFT); 3862 .LM690: 3863 12e4 80E0 ldi r24,lo8(0) 3864 12e6 0E94 0000 call Align 1427:srx1.c **** 1428:srx1.c **** // Rotate Right 90 degrees 1429:srx1.c **** Rotate(+90, FALSE); 3866 .LM691: 3867 12ea 60E0 ldi r22,lo8(0) 3868 12ec 8AE5 ldi r24,lo8(90) 3869 12ee 90E0 ldi r25,hi8(90) 3870 12f0 0E94 0000 call Rotate 1430:srx1.c **** 1431:srx1.c **** // Align with Left wall 1432:srx1.c **** Align(ALIGN_LEFT); 3872 .LM692: 3873 12f4 80E0 ldi r24,lo8(0) 3874 12f6 0E94 0000 call Align 1433:srx1.c **** 1434:srx1.c **** // TODO: Use follow instead and implement a 'distance traveled' option. (darn floor items!) 1435:srx1.c **** 1436:srx1.c **** // Move forward to entrance. 1437:srx1.c **** Forward(48, FORWARD_REPORT_SUCCESS); 3876 .LM693: 3877 12fa 6C2F mov r22,r28 3878 12fc 80E3 ldi r24,lo8(48) 3879 12fe 0E94 0000 call Forward 1438:srx1.c **** 1439:srx1.c **** // Align with Left wall 1440:srx1.c **** Align(ALIGN_LEFT); 3881 .LM694: 3882 1302 80E0 ldi r24,lo8(0) 3883 1304 0E94 0000 call Align 1441:srx1.c **** 1442:srx1.c **** // Rotate Right 90 degrees 1443:srx1.c **** Rotate(+90, FALSE); 3885 .LM695: 3886 1308 60E0 ldi r22,lo8(0) 3887 130a 8AE5 ldi r24,lo8(90) 3888 130c 90E0 ldi r25,hi8(90) 3889 130e 0E94 0000 call Rotate 1444:srx1.c **** 1445:srx1.c **** // Move forward to entrance. 1446:srx1.c **** Forward(0, FORWARD_REPORT_DOOR); 3891 .LM696: 3892 1312 602F mov r22,r16 3893 1314 80E0 ldi r24,lo8(0) 3894 1316 0E94 0000 call Forward 1447:srx1.c **** 1448:srx1.c **** // Align with door frame 1449:srx1.c **** //Align(ALIGN_DOOR); 1450:srx1.c **** 1451:srx1.c **** // We're in room #4 1452:srx1.c **** Room = 4; 3896 .LM697: 3897 131a 84E0 ldi r24,lo8(4) 3898 131c 8093 0000 sts Room,r24 1453:srx1.c **** 1454:srx1.c **** // Configure SEARCH to search current room. 1455:srx1.c **** if (Search(LEFT) == TRUE) 3900 .LM698: 3901 1320 80E0 ldi r24,lo8(0) 3902 1322 0E94 0000 call Search 3903 1326 8130 cpi r24,lo8(1) 3904 1328 11F4 brne .L234 1456:srx1.c **** GoHome(); 3906 .LM699: 3907 132a 0E94 0000 call GoHome 3908 .L234: 1457:srx1.c **** 1458:srx1.c **** // TODO: Add necessary calls to have robot get back to home and prepare to go over again.. 1459:srx1.c **** //goto START; 1460:srx1.c **** 1461:srx1.c **** // If we get here it means we suck! 1462:srx1.c **** AvrXSuspend(PID(planner)); 3910 .LM700: 3911 132e 80E0 ldi r24,lo8(plannerPid) 3912 1330 90E0 ldi r25,hi8(plannerPid) 3913 1332 0E94 0000 call AvrXSuspend 1463:srx1.c **** } 3915 .LM701: 3916 /* epilogue: frame size=0 */ 3917 /* epilogue: naked */ 3918 /* epilogue end (size=0) */ 3919 /* function planner size 268 (268) */ 3921 .Lscope2: 3923 .lcomm ArbitrateCmd.8,14 3925 .global forward 3927 forward: 1464:srx1.c **** 1465:srx1.c **** 1466:srx1.c **** 1467:srx1.c **** /****************************************** 1468:srx1.c **** Task - Forward (Move forward/Backwards) 1469:srx1.c **** ----------------------------- 1470:srx1.c **** Inputs: lenght argument contains vectorial info (if 0, keep forward until obstacle detected). 1471:srx1.c **** 1472:srx1.c **** ******************************************/ 1473:srx1.c **** NAKEDFUNC(forward) 1474:srx1.c **** { 3929 .LM702: 3930 /* prologue: frame size=0 */ 3931 /* prologue: naked */ 3932 /* prologue end (size=0) */ 1475:srx1.c **** MessageControlBlock *pForwardCmd; 1476:srx1.c **** static ArbitrateMessage ArbitrateCmd; // static is required. 1477:srx1.c **** int iLenght; 1478:srx1.c **** unsigned char line_sense_l, 1479:srx1.c **** line_sense_r; 1480:srx1.c **** unsigned char exit; 1481:srx1.c **** 1482:srx1.c **** // Identify ourselves 1483:srx1.c **** ArbitrateCmd.id = FORWARD; 3934 .LM703: 3935 .LBB4: 3936 1336 83E0 ldi r24,lo8(3) 3937 1338 8093 0000 sts ArbitrateCmd.8+4,r24 3938 .L257: 1484:srx1.c **** 1485:srx1.c **** while (1) 1486:srx1.c **** { 1487:srx1.c **** pForwardCmd = AvrXWaitMessage(&ForwardQueue); 3940 .LM704: 3941 133c 80E0 ldi r24,lo8(ForwardQueue) 3942 133e 90E0 ldi r25,hi8(ForwardQueue) 3943 1340 0E94 0000 call AvrXWaitMessage 3944 1344 8C01 movw r16,r24 1488:srx1.c **** 1489:srx1.c **** // setup 1490:srx1.c **** exit = FALSE; 3946 .LM705: 3947 1346 EE24 clr r14 1491:srx1.c **** ArbitrateCmd.info.active = TRUE; 3949 .LM706: 3950 1348 81E0 ldi r24,lo8(1) 3951 134a 8093 0000 sts ArbitrateCmd.8+6,r24 1492:srx1.c **** ArbitrateCmd.info.type = ABSOLUTE; 3953 .LM707: 3954 134e 8093 0000 sts ArbitrateCmd.8+13,r24 1493:srx1.c **** last_line_trigger = 0; 3956 .LM708: 3957 1352 E092 0000 sts last_line_trigger,r14 1494:srx1.c **** 1495:srx1.c **** // Loop Counter = distance / (DIST_TICK * Velocity) 1496:srx1.c **** iLenght = ((ForwardMessage*)pForwardCmd)->lenght / (DIST_PER_TICK * FORWARD_SPEED); 3959 .LM709: 3960 1356 F801 movw r30,r16 3961 1358 8481 ldd r24,Z+4 3962 135a 9581 ldd r25,Z+5 3963 135c AA27 clr r26 3964 135e 97FD sbrc r25,7 3965 1360 A095 com r26 3966 1362 BA2F mov r27,r26 3967 1364 BC01 movw r22,r24 3968 1366 CD01 movw r24,r26 3969 1368 0E94 0000 call __floatsisf 3970 136c DC01 movw r26,r24 3971 136e CB01 movw r24,r22 3972 1370 28E9 ldi r18,lo8(0x3f12bf98) 3973 1372 3FEB ldi r19,hi8(0x3f12bf98) 3974 1374 42E1 ldi r20,hlo8(0x3f12bf98) 3975 1376 5FE3 ldi r21,hhi8(0x3f12bf98) 3976 1378 BC01 movw r22,r24 3977 137a CD01 movw r24,r26 3978 137c 0E94 0000 call __divsf3 3979 1380 DC01 movw r26,r24 3980 1382 CB01 movw r24,r22 3981 1384 BC01 movw r22,r24 3982 1386 CD01 movw r24,r26 3983 1388 0E94 0000 call __fixsfsi 3984 138c DC01 movw r26,r24 3985 138e CB01 movw r24,r22 3986 1390 EC01 movw r28,r24 1497:srx1.c **** 1498:srx1.c **** if (iLenght >= 0) // Covers continous case (ex. lenght argument = 0) 3988 .LM710: 3989 1392 97FD sbrc r25,7 3990 1394 0BC0 rjmp .L239 1499:srx1.c **** { 1500:srx1.c **** ArbitrateCmd.info.output_l = FORWARD_SPEED; 3992 .LM711: 3993 1396 82E2 ldi r24,lo8(34) 3994 1398 90E0 ldi r25,hi8(34) 3995 139a 9093 0000 sts (ArbitrateCmd.8+8)+1,r25 3996 139e 8093 0000 sts ArbitrateCmd.8+8,r24 1501:srx1.c **** ArbitrateCmd.info.output_r = FORWARD_SPEED; 3998 .LM712: 3999 13a2 9093 0000 sts (ArbitrateCmd.8+10)+1,r25 4000 13a6 8093 0000 sts ArbitrateCmd.8+10,r24 4001 13aa 0DC0 rjmp .L240 4002 .L239: 1502:srx1.c **** } 1503:srx1.c **** else 1504:srx1.c **** { 1505:srx1.c **** ArbitrateCmd.info.output_l = -FORWARD_SPEED; 4004 .LM713: 4005 13ac 8EED ldi r24,lo8(-34) 4006 13ae 9FEF ldi r25,hi8(-34) 4007 13b0 9093 0000 sts (ArbitrateCmd.8+8)+1,r25 4008 13b4 8093 0000 sts ArbitrateCmd.8+8,r24 1506:srx1.c **** ArbitrateCmd.info.output_r = -FORWARD_SPEED; 4010 .LM714: 4011 13b8 9093 0000 sts (ArbitrateCmd.8+10)+1,r25 4012 13bc 8093 0000 sts ArbitrateCmd.8+10,r24 1507:srx1.c **** iLenght = -iLenght; 4014 .LM715: 4015 13c0 D095 com r29 4016 13c2 C195 neg r28 4017 13c4 DF4F sbci r29,lo8(-1) 4018 .L240: 1508:srx1.c **** } 1509:srx1.c **** 1510:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 4020 .LM716: 4021 13c6 60E0 ldi r22,lo8(ArbitrateCmd.8) 4022 13c8 70E0 ldi r23,hi8(ArbitrateCmd.8) 4023 13ca 80E0 ldi r24,lo8(ArbitrateQueue) 4024 13cc 90E0 ldi r25,hi8(ArbitrateQueue) 4025 13ce 0E94 0000 call AvrXSendMessage 1511:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 4027 .LM717: 4028 13d2 80E0 ldi r24,lo8(ArbitrateCmd.8) 4029 13d4 90E0 ldi r25,hi8(ArbitrateCmd.8) 4030 13d6 0E94 0000 call AvrXWaitMessageAck 1512:srx1.c **** 1513:srx1.c **** // TODO: Have different speed constants for both cases? 1514:srx1.c **** 1515:srx1.c **** if (iLenght == 0) // continuous case 4032 .LM718: 4033 13da 2097 sbiw r28,0 4034 13dc A1F5 brne .L241 1516:srx1.c **** { 1517:srx1.c **** while (exit != TRUE) 4036 .LM719: 4037 13de F1E0 ldi r31,lo8(1) 4038 13e0 EF16 cp r14,r31 4039 13e2 09F4 brne .+2 4040 13e4 3FC0 rjmp .L252 4041 .L251: 1518:srx1.c **** { 1519:srx1.c **** SynchWithMotion(); 4043 .LM720: 4044 13e6 0E94 0000 call SynchWithMotion 1520:srx1.c **** 1521:srx1.c **** line_sense_l = adc_read(LINE_SENSOR_L); 4046 .LM721: 4047 13ea 86E0 ldi r24,lo8(6) 4048 13ec 0E94 0000 call adc_read 4049 13f0 C82F mov r28,r24 1522:srx1.c **** line_sense_r = adc_read(LINE_SENSOR_R); 4051 .LM722: 4052 13f2 87E0 ldi r24,lo8(7) 4053 13f4 0E94 0000 call adc_read 4054 13f8 F82E mov r15,r24 1523:srx1.c **** 1524:srx1.c **** // debug 1525:srx1.c **** //PutCRLF(); 1526:srx1.c **** //PutDecWord(line_sense_l); 1527:srx1.c **** //PutChar('-'); 1528:srx1.c **** //PutDecWord(line_sense_r); 1529:srx1.c **** 1530:srx1.c **** if (gpd_read(GPD_F) < FORWARD_FRONT_THRESHOLD) 4056 .LM723: 4057 13fa 80E0 ldi r24,lo8(0) 4058 13fc 0E94 0000 call gpd_read 4059 1400 8031 cpi r24,lo8(16) 4060 1402 10F4 brsh .L245 1531:srx1.c **** { 1532:srx1.c **** ((ForwardMessage*)pForwardCmd)->status = FORWARD_REPORT_OBSTACLE; // report back. 4062 .LM724: 4063 1404 82E0 ldi r24,lo8(2) 4064 1406 17C0 rjmp .L262 4065 .L245: 1533:srx1.c **** exit = TRUE; 1534:srx1.c **** } 1535:srx1.c **** else 1536:srx1.c **** if ((((ForwardMessage*)pForwardCmd)->ignore_door != TRUE) && (LINE_SENSOR(line_sense_l) | 4067 .LM725: 4068 1408 F801 movw r30,r16 4069 140a 8681 ldd r24,Z+6 4070 140c 8130 cpi r24,lo8(1) 4071 140e B9F0 breq .L242 4072 1410 9091 0000 lds r25,R_ParameterTable+6 4073 1414 9C17 cp r25,r28 4074 1416 10F0 brlo .L248 4075 1418 9F15 cp r25,r15 4076 141a 88F4 brsh .L242 4077 .L248: 1537:srx1.c **** { 1538:srx1.c **** // Keep track which sensor fired first 1539:srx1.c **** if (LINE_SENSOR(line_sense_l)) 4079 .LM726: 4080 141c 9C17 cp r25,r28 4081 141e 18F4 brsh .L249 1540:srx1.c **** last_line_trigger = LINE_SENSOR_LEFT; 4083 .LM727: 4084 1420 81E0 ldi r24,lo8(1) 4085 1422 8093 0000 sts last_line_trigger,r24 4086 .L249: 1541:srx1.c **** 1542:srx1.c **** if (LINE_SENSOR(line_sense_r)) 4088 .LM728: 4089 1426 9F15 cp r25,r15 4090 1428 28F4 brsh .L250 1543:srx1.c **** last_line_trigger += LINE_SENSOR_RIGHT; // Special case: if both sensors triggered simultaneo 4092 .LM729: 4093 142a 8091 0000 lds r24,last_line_trigger 4094 142e 8E5F subi r24,lo8(-(2)) 4095 1430 8093 0000 sts last_line_trigger,r24 4096 .L250: 1544:srx1.c **** 1545:srx1.c **** ((ForwardMessage*)pForwardCmd)->status = FORWARD_REPORT_DOOR; // report back. 4098 .LM730: 4099 1434 83E0 ldi r24,lo8(3) 4100 .L262: 4101 1436 F801 movw r30,r16 4102 1438 8783 std Z+7,r24 1546:srx1.c **** //EmergencyStop(); // Slam the breaks hard! 1547:srx1.c **** exit = TRUE; 4104 .LM731: 4105 143a 41E0 ldi r20,lo8(1) 4106 143c E42E mov r14,r20 4107 .L242: 4108 143e F1E0 ldi r31,lo8(1) 4109 1440 EF16 cp r14,r31 4110 1442 89F6 brne .L251 4111 1444 0FC0 rjmp .L252 4112 .L241: 1548:srx1.c **** } 1549:srx1.c **** } 1550:srx1.c **** } 1551:srx1.c **** else // Fixed case 1552:srx1.c **** { 1553:srx1.c **** while (iLenght--) // TODO: Add front sensor reading in here too? 4114 .LM732: 4115 1446 2197 sbiw r28,1 4116 1448 8FEF ldi r24,hi8(-1) 4117 144a CF3F cpi r28,lo8(-1) 4118 144c D807 cpc r29,r24 4119 144e 39F0 breq .L261 4120 .L256: 1554:srx1.c **** SynchWithMotion(); 4122 .LM733: 4123 1450 0E94 0000 call SynchWithMotion 4124 1454 2197 sbiw r28,1 4125 1456 EFEF ldi r30,hi8(-1) 4126 1458 CF3F cpi r28,lo8(-1) 4127 145a DE07 cpc r29,r30 4128 145c C9F7 brne .L256 4129 .L261: 1555:srx1.c **** 1556:srx1.c **** ((ForwardMessage*)pForwardCmd)->status = FORWARD_REPORT_SUCCESS; // report back. 4131 .LM734: 4132 145e 81E0 ldi r24,lo8(1) 4133 1460 F801 movw r30,r16 4134 1462 8783 std Z+7,r24 4135 .L252: 1557:srx1.c **** } 1558:srx1.c **** 1559:srx1.c **** // Stop motion and de-assert ourselves 1560:srx1.c **** ArbitrateCmd.info.output_l = POWER(0); 4137 .LM735: 4138 1464 1092 0000 sts (ArbitrateCmd.8+8)+1,__zero_reg__ 4139 1468 1092 0000 sts ArbitrateCmd.8+8,__zero_reg__ 1561:srx1.c **** ArbitrateCmd.info.output_r = POWER(0); 4141 .LM736: 4142 146c 1092 0000 sts (ArbitrateCmd.8+10)+1,__zero_reg__ 4143 1470 1092 0000 sts ArbitrateCmd.8+10,__zero_reg__ 1562:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 4145 .LM737: 4146 1474 60E0 ldi r22,lo8(ArbitrateCmd.8) 4147 1476 70E0 ldi r23,hi8(ArbitrateCmd.8) 4148 1478 80E0 ldi r24,lo8(ArbitrateQueue) 4149 147a 90E0 ldi r25,hi8(ArbitrateQueue) 4150 147c 0E94 0000 call AvrXSendMessage 1563:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 4152 .LM738: 4153 1480 80E0 ldi r24,lo8(ArbitrateCmd.8) 4154 1482 90E0 ldi r25,hi8(ArbitrateCmd.8) 4155 1484 0E94 0000 call AvrXWaitMessageAck 1564:srx1.c **** 1565:srx1.c **** ArbitrateCmd.info.active = FALSE; 4157 .LM739: 4158 1488 1092 0000 sts ArbitrateCmd.8+6,__zero_reg__ 1566:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 4160 .LM740: 4161 148c 60E0 ldi r22,lo8(ArbitrateCmd.8) 4162 148e 70E0 ldi r23,hi8(ArbitrateCmd.8) 4163 1490 80E0 ldi r24,lo8(ArbitrateQueue) 4164 1492 90E0 ldi r25,hi8(ArbitrateQueue) 4165 1494 0E94 0000 call AvrXSendMessage 1567:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 4167 .LM741: 4168 1498 80E0 ldi r24,lo8(ArbitrateCmd.8) 4169 149a 90E0 ldi r25,hi8(ArbitrateCmd.8) 4170 149c 0E94 0000 call AvrXWaitMessageAck 1568:srx1.c **** 1569:srx1.c **** // report back! 1570:srx1.c **** AvrXAckMessage(pForwardCmd); 4172 .LM742: 4173 14a0 C801 movw r24,r16 4174 14a2 0E94 0000 call AvrXAckMessage 4175 14a6 4ACF rjmp .L257 1571:srx1.c **** } 1572:srx1.c **** } 4177 .LM743: 4178 .LBE4: 4179 /* epilogue: frame size=0 */ 4180 /* epilogue: naked */ 4181 /* epilogue end (size=0) */ 4182 /* function forward size 184 (184) */ 4192 .Lscope3: 4194 .lcomm ArbitrateCmd.9,14 4196 .global avoid 4198 avoid: 1573:srx1.c **** 1574:srx1.c **** /****************************************** 1575:srx1.c **** Task - avoid 1576:srx1.c **** ******************************************/ 1577:srx1.c **** NAKEDFUNC(avoid) 1578:srx1.c **** { 4200 .LM744: 4201 /* prologue: frame size=0 */ 4202 /* prologue: naked */ 4203 /* prologue end (size=0) */ 1579:srx1.c **** //MessageControlBlock *pAvoidCmd; 1580:srx1.c **** static ArbitrateMessage ArbitrateCmd; // static is required. 1581:srx1.c **** unsigned char state; 1582:srx1.c **** unsigned char laststate; 1583:srx1.c **** unsigned char d_front, 1584:srx1.c **** d_l_f, 1585:srx1.c **** d_r_f; 1586:srx1.c **** 1587:srx1.c **** 1588:srx1.c **** // Reduce code by initializing it once only. 1589:srx1.c **** ArbitrateCmd.id=AVOID; 4205 .LM745: 4206 .LBB5: 4207 14a8 85E0 ldi r24,lo8(5) 4208 14aa 8093 0000 sts ArbitrateCmd.9+4,r24 1590:srx1.c **** ArbitrateCmd.info.type = ABSOLUTE; 4210 .LM746: 4211 14ae 81E0 ldi r24,lo8(1) 4212 14b0 8093 0000 sts ArbitrateCmd.9+13,r24 1591:srx1.c **** laststate = FALSE; 4214 .LM747: 4215 14b4 00E0 ldi r16,lo8(0) 4216 .L273: 1592:srx1.c **** 1593:srx1.c **** while (1) 1594:srx1.c **** { 1595:srx1.c **** //pAvoidCmd = AvrXWaitMessage(&AvoidQueue); 1596:srx1.c **** 1597:srx1.c **** // Synch up with motion control 1598:srx1.c **** //SynchWithMotion(); 1599:srx1.c **** 1600:srx1.c **** d_front = gpd_read(GPD_F); 4218 .LM748: 4219 14b6 80E0 ldi r24,lo8(0) 4220 14b8 0E94 0000 call gpd_read 4221 14bc 182F mov r17,r24 1601:srx1.c **** d_l_f = gpd_read(GPD_L_F); 4223 .LM749: 4224 14be 81E0 ldi r24,lo8(1) 4225 14c0 0E94 0000 call gpd_read 4226 14c4 C82F mov r28,r24 1602:srx1.c **** d_r_f = gpd_read(GPD_R_F); 4228 .LM750: 4229 14c6 83E0 ldi r24,lo8(3) 4230 14c8 0E94 0000 call gpd_read 1603:srx1.c **** 1604:srx1.c **** if ( d_front < AVOID_FRONT_THRESHOLD || d_l_f < AVOID_SIDE_THRESHOLD || d_r_f < AVOID_SIDE_THRESH 4232 .LM751: 4233 14cc 1F30 cpi r17,lo8(15) 4234 14ce 20F0 brlo .L268 4235 14d0 C330 cpi r28,lo8(3) 4236 14d2 10F0 brlo .L268 4237 14d4 8330 cpi r24,lo8(3) 4238 14d6 D8F4 brsh .L267 4239 .L268: 1605:srx1.c **** { 1606:srx1.c **** state = TRUE; // To prevent redundant messages 4241 .LM752: 4242 14d8 21E0 ldi r18,lo8(1) 1607:srx1.c **** ArbitrateCmd.info.active = TRUE; 4244 .LM753: 4245 14da 2093 0000 sts ArbitrateCmd.9+6,r18 1608:srx1.c **** 1609:srx1.c **** if (d_l_f > d_r_f) 4247 .LM754: 4248 14de 8C17 cp r24,r28 4249 14e0 48F4 brsh .L269 1610:srx1.c **** { 1611:srx1.c **** ArbitrateCmd.info.output_r = AVOID_TURN_SPEED; 4251 .LM755: 4252 14e2 8CE0 ldi r24,lo8(12) 4253 14e4 90E0 ldi r25,hi8(12) 4254 14e6 9093 0000 sts (ArbitrateCmd.9+10)+1,r25 4255 14ea 8093 0000 sts ArbitrateCmd.9+10,r24 1612:srx1.c **** ArbitrateCmd.info.output_l = -AVOID_TURN_SPEED; 4257 .LM756: 4258 14ee 84EF ldi r24,lo8(-12) 4259 14f0 9FEF ldi r25,hi8(-12) 4260 14f2 08C0 rjmp .L274 4261 .L269: 1613:srx1.c **** } 1614:srx1.c **** else 1615:srx1.c **** { 1616:srx1.c **** ArbitrateCmd.info.output_r = -AVOID_TURN_SPEED; 4263 .LM757: 4264 14f4 84EF ldi r24,lo8(-12) 4265 14f6 9FEF ldi r25,hi8(-12) 4266 14f8 9093 0000 sts (ArbitrateCmd.9+10)+1,r25 4267 14fc 8093 0000 sts ArbitrateCmd.9+10,r24 1617:srx1.c **** ArbitrateCmd.info.output_l = AVOID_TURN_SPEED; 4269 .LM758: 4270 1500 8CE0 ldi r24,lo8(12) 4271 1502 90E0 ldi r25,hi8(12) 4272 .L274: 4273 1504 9093 0000 sts (ArbitrateCmd.9+8)+1,r25 4274 1508 8093 0000 sts ArbitrateCmd.9+8,r24 4275 150c 0BC0 rjmp .L271 4276 .L267: 1618:srx1.c **** } 1619:srx1.c **** } 1620:srx1.c **** else 1621:srx1.c **** { 1622:srx1.c **** state = FALSE; // To prevent redundant messages 4278 .LM759: 4279 150e 20E0 ldi r18,lo8(0) 1623:srx1.c **** ArbitrateCmd.info.active = FALSE; 4281 .LM760: 4282 1510 2093 0000 sts ArbitrateCmd.9+6,r18 1624:srx1.c **** ArbitrateCmd.info.output_l = POWER(0); 4284 .LM761: 4285 1514 1092 0000 sts (ArbitrateCmd.9+8)+1,__zero_reg__ 4286 1518 1092 0000 sts ArbitrateCmd.9+8,__zero_reg__ 1625:srx1.c **** ArbitrateCmd.info.output_r = POWER(0); 4288 .LM762: 4289 151c 1092 0000 sts (ArbitrateCmd.9+10)+1,__zero_reg__ 4290 1520 1092 0000 sts ArbitrateCmd.9+10,__zero_reg__ 4291 .L271: 1626:srx1.c **** } 1627:srx1.c **** 1628:srx1.c **** 1629:srx1.c **** // Update our arbitration status (only if there was any changes since last update) 1630:srx1.c **** if (state != laststate) 4293 .LM763: 4294 1524 2017 cp r18,r16 4295 1526 59F0 breq .L272 1631:srx1.c **** { 1632:srx1.c **** laststate = state; //Update for next loop. 4297 .LM764: 4298 1528 022F mov r16,r18 1633:srx1.c **** 1634:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 4300 .LM765: 4301 152a 60E0 ldi r22,lo8(ArbitrateCmd.9) 4302 152c 70E0 ldi r23,hi8(ArbitrateCmd.9) 4303 152e 80E0 ldi r24,lo8(ArbitrateQueue) 4304 1530 90E0 ldi r25,hi8(ArbitrateQueue) 4305 1532 0E94 0000 call AvrXSendMessage 1635:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 4307 .LM766: 4308 1536 80E0 ldi r24,lo8(ArbitrateCmd.9) 4309 1538 90E0 ldi r25,hi8(ArbitrateCmd.9) 4310 153a 0E94 0000 call AvrXWaitMessageAck 4311 .L272: 1636:srx1.c **** } 1637:srx1.c **** 1638:srx1.c **** // report back! 1639:srx1.c **** //AvrXAckMessage(pAvoidCmd); 1640:srx1.c **** 1641:srx1.c **** AvrXDelay(&avoid_timer, AVOID_LOOP_DELAY); 4313 .LM767: 4314 153e 60EF ldi r22,lo8(240) 4315 1540 70E0 ldi r23,hi8(240) 4316 1542 80E0 ldi r24,lo8(avoid_timer) 4317 1544 90E0 ldi r25,hi8(avoid_timer) 4318 1546 0E94 0000 call AvrXDelay 4319 154a B5CF rjmp .L273 1642:srx1.c **** } 1643:srx1.c **** } 4321 .LM768: 4322 .LBE5: 4323 /* epilogue: frame size=0 */ 4324 /* epilogue: naked */ 4325 /* epilogue end (size=0) */ 4326 /* function avoid size 82 (82) */ 4336 .Lscope4: 4338 .lcomm ArbitrateCmd.10,14 4340 .global rotate 4342 rotate: 1644:srx1.c **** 1645:srx1.c **** 1646:srx1.c **** 1647:srx1.c **** /****************************************** 1648:srx1.c **** Task - rotate 1649:srx1.c **** ******************************************/ 1650:srx1.c **** NAKEDFUNC(rotate) 1651:srx1.c **** { 4344 .LM769: 4345 /* prologue: frame size=0 */ 4346 /* prologue: naked */ 4347 /* prologue end (size=0) */ 1652:srx1.c **** MessageControlBlock *pRotateCmd; 1653:srx1.c **** static ArbitrateMessage ArbitrateCmd; // static is required. 1654:srx1.c **** int iTurn; 1655:srx1.c **** int sign; 1656:srx1.c **** 1657:srx1.c **** // Reduce code by initializing it once only. 1658:srx1.c **** ArbitrateCmd.id=ROTATE; 4349 .LM770: 4350 .LBB6: 4351 154c 82E0 ldi r24,lo8(2) 4352 154e 8093 0000 sts ArbitrateCmd.10+4,r24 1659:srx1.c **** ArbitrateCmd.info.type = ABSOLUTE; 4354 .LM771: 4355 1552 81E0 ldi r24,lo8(1) 4356 1554 8093 0000 sts ArbitrateCmd.10+13,r24 1660:srx1.c **** ArbitrateCmd.info.precision = TRUE; // request right to pass non-normalized values to keep preci 4358 .LM772: 4359 1558 8093 0000 sts ArbitrateCmd.10+12,r24 4360 .L294: 1661:srx1.c **** 1662:srx1.c **** while (1) 1663:srx1.c **** { 1664:srx1.c **** pRotateCmd = AvrXWaitMessage(&RotateQueue); 4362 .LM773: 4363 155c 80E0 ldi r24,lo8(RotateQueue) 4364 155e 90E0 ldi r25,hi8(RotateQueue) 4365 1560 0E94 0000 call AvrXWaitMessage 4366 1564 8C01 movw r16,r24 1665:srx1.c **** 1666:srx1.c **** // Take over 1667:srx1.c **** ArbitrateCmd.info.active = TRUE; 4368 .LM774: 4369 1566 81E0 ldi r24,lo8(1) 4370 1568 8093 0000 sts ArbitrateCmd.10+6,r24 1668:srx1.c **** ((RotateMessage*)pRotateCmd)->status = FALSE; // Init just in case. 4372 .LM775: 4373 156c F801 movw r30,r16 4374 156e 1086 std Z+8,__zero_reg__ 1669:srx1.c **** 1670:srx1.c **** // Make sure we're totally stopped before starting to keep precision high 1671:srx1.c **** ArbitrateCmd.info.output_l = 0; 4376 .LM776: 4377 1570 1092 0000 sts (ArbitrateCmd.10+8)+1,__zero_reg__ 4378 1574 1092 0000 sts ArbitrateCmd.10+8,__zero_reg__ 1672:srx1.c **** ArbitrateCmd.info.output_r = 0; 4380 .LM777: 4381 1578 1092 0000 sts (ArbitrateCmd.10+10)+1,__zero_reg__ 4382 157c 1092 0000 sts ArbitrateCmd.10+10,__zero_reg__ 1673:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 4384 .LM778: 4385 1580 60E0 ldi r22,lo8(ArbitrateCmd.10) 4386 1582 70E0 ldi r23,hi8(ArbitrateCmd.10) 4387 1584 80E0 ldi r24,lo8(ArbitrateQueue) 4388 1586 90E0 ldi r25,hi8(ArbitrateQueue) 4389 1588 0E94 0000 call AvrXSendMessage 1674:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 4391 .LM779: 4392 158c 80E0 ldi r24,lo8(ArbitrateCmd.10) 4393 158e 90E0 ldi r25,hi8(ArbitrateCmd.10) 4394 1590 0E94 0000 call AvrXWaitMessageAck 1675:srx1.c **** WaitForMotionToStop(); 4396 .LM780: 4397 1594 0E94 0000 call WaitForMotionToStop 1676:srx1.c **** 1677:srx1.c **** // Was requested rotation absolute or relative? 1678:srx1.c **** if (((RotateMessage*)pRotateCmd)->type == ABSOLUTE) 4399 .LM781: 4400 1598 F801 movw r30,r16 4401 159a 8681 ldd r24,Z+6 4402 159c 2481 ldd r18,Z+4 4403 159e 3581 ldd r19,Z+5 4404 15a0 8130 cpi r24,lo8(1) 4405 15a2 71F5 brne .L279 1679:srx1.c **** { 1680:srx1.c **** // Normalize input (force within valid 0-359 range) 1681:srx1.c **** iTurn = ((RotateMessage*)pRotateCmd)->angle - iTheta; 4407 .LM782: 4408 15a4 8091 0000 lds r24,iTheta 4409 15a8 9091 0000 lds r25,(iTheta)+1 4410 15ac E901 movw r28,r18 4411 15ae C81B sub r28,r24 4412 15b0 D90B sbc r29,r25 1682:srx1.c **** sign = (iTurn > 0) ? 1 : -1; 4414 .LM783: 4415 15b2 1C16 cp __zero_reg__,r28 4416 15b4 1D06 cpc __zero_reg__,r29 4417 15b6 1CF4 brge .L280 4418 15b8 21E0 ldi r18,lo8(1) 4419 15ba 30E0 ldi r19,hi8(1) 4420 15bc 02C0 rjmp .L281 4421 .L280: 4422 15be 2FEF ldi r18,lo8(-1) 4423 15c0 3FEF ldi r19,hi8(-1) 4424 .L281: 1683:srx1.c **** 1684:srx1.c **** iTurn = abs(iTurn) % 360; 4426 .LM784: 4427 15c2 CE01 movw r24,r28 4428 15c4 D7FF sbrs r29,7 4429 15c6 04C0 rjmp .L282 4430 15c8 8827 clr r24 4431 15ca 9927 clr r25 4432 15cc 8C1B sub r24,r28 4433 15ce 9D0B sbc r25,r29 4434 .L282: 4435 15d0 E8E6 ldi r30,lo8(360) 4436 15d2 F1E0 ldi r31,hi8(360) 4437 15d4 BF01 movw r22,r30 4438 15d6 0E94 0000 call __divmodhi4 4439 15da EC01 movw r28,r24 1685:srx1.c **** 1686:srx1.c **** // pick shortest route around the circle 1687:srx1.c **** if (iTurn > 180) 4441 .LM785: 4442 15dc 853B cpi r24,181 4443 15de 9105 cpc r25,__zero_reg__ 4444 15e0 34F0 brlt .L283 1688:srx1.c **** { 1689:srx1.c **** iTurn = 360 - iTurn; 4446 .LM786: 4447 15e2 E81B sub r30,r24 4448 15e4 F90B sbc r31,r25 4449 15e6 EF01 movw r28,r30 1690:srx1.c **** sign = -sign; 4451 .LM787: 4452 15e8 3095 com r19 4453 15ea 2195 neg r18 4454 15ec 3F4F sbci r19,lo8(-1) 4455 .L283: 1691:srx1.c **** } 1692:srx1.c **** 1693:srx1.c **** iTurn = iTurn * sign; 4457 .LM788: 4458 15ee C29F mul r28,r18 4459 15f0 C001 movw r24,r0 4460 15f2 C39F mul r28,r19 4461 15f4 900D add r25,r0 4462 15f6 D29F mul r29,r18 4463 15f8 900D add r25,r0 4464 15fa 1124 clr r1 4465 15fc EC01 movw r28,r24 4466 15fe 01C0 rjmp .L284 4467 .L279: 1694:srx1.c **** } 1695:srx1.c **** else // RELATIVE 1696:srx1.c **** iTurn = ((RotateMessage*)pRotateCmd)->angle; 4469 .LM789: 4470 1600 E901 movw r28,r18 4471 .L284: 1697:srx1.c **** 1698:srx1.c **** #define ANGLE_ADJUST -1 1699:srx1.c **** 1700:srx1.c **** // TODO: Implement a lookup table of pre-calculated angles to gain precision. 1701:srx1.c **** if (iTurn > 0) // Start new motion 4473 .LM790: 4474 1602 1C16 cp __zero_reg__,r28 4475 1604 1D06 cpc __zero_reg__,r29 4476 1606 6CF4 brge .L285 1702:srx1.c **** { 1703:srx1.c **** ArbitrateCmd.info.output_l = (ONE_DEG-ANGLE_ADJUST)<<8; 4478 .LM791: 4479 1608 80E0 ldi r24,lo8(3584) 4480 160a 9EE0 ldi r25,hi8(3584) 4481 160c 9093 0000 sts (ArbitrateCmd.10+8)+1,r25 4482 1610 8093 0000 sts ArbitrateCmd.10+8,r24 1704:srx1.c **** ArbitrateCmd.info.output_r = (-ONE_DEG+ANGLE_ADJUST)<<8; 4484 .LM792: 4485 1614 80E0 ldi r24,lo8(-3584) 4486 1616 92EF ldi r25,hi8(-3584) 4487 1618 9093 0000 sts (ArbitrateCmd.10+10)+1,r25 4488 161c 8093 0000 sts ArbitrateCmd.10+10,r24 4489 1620 0FC0 rjmp .L286 4490 .L285: 1705:srx1.c **** } 1706:srx1.c **** else 1707:srx1.c **** { 1708:srx1.c **** ArbitrateCmd.info.output_l = (-ONE_DEG+ANGLE_ADJUST)<<8; 4492 .LM793: 4493 1622 80E0 ldi r24,lo8(-3584) 4494 1624 92EF ldi r25,hi8(-3584) 4495 1626 9093 0000 sts (ArbitrateCmd.10+8)+1,r25 4496 162a 8093 0000 sts ArbitrateCmd.10+8,r24 1709:srx1.c **** ArbitrateCmd.info.output_r = (ONE_DEG-ANGLE_ADJUST)<<8; 4498 .LM794: 4499 162e 80E0 ldi r24,lo8(3584) 4500 1630 9EE0 ldi r25,hi8(3584) 4501 1632 9093 0000 sts (ArbitrateCmd.10+10)+1,r25 4502 1636 8093 0000 sts ArbitrateCmd.10+10,r24 1710:srx1.c **** iTurn = -iTurn; 4504 .LM795: 4505 163a D095 com r29 4506 163c C195 neg r28 4507 163e DF4F sbci r29,lo8(-1) 4508 .L286: 1711:srx1.c **** } 1712:srx1.c **** 1713:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 4510 .LM796: 4511 1640 60E0 ldi r22,lo8(ArbitrateCmd.10) 4512 1642 70E0 ldi r23,hi8(ArbitrateCmd.10) 4513 1644 80E0 ldi r24,lo8(ArbitrateQueue) 4514 1646 90E0 ldi r25,hi8(ArbitrateQueue) 4515 1648 0E94 0000 call AvrXSendMessage 1714:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 4517 .LM797: 4518 164c 80E0 ldi r24,lo8(ArbitrateCmd.10) 4519 164e 90E0 ldi r25,hi8(ArbitrateCmd.10) 4520 1650 0E94 0000 call AvrXWaitMessageAck 1715:srx1.c **** 1716:srx1.c **** while (iTurn--) 4522 .LM798: 4523 1654 2197 sbiw r28,1 4524 1656 9FEF ldi r25,hi8(-1) 4525 1658 CF3F cpi r28,lo8(-1) 4526 165a D907 cpc r29,r25 4527 165c C9F0 breq .L288 4528 .L293: 1717:srx1.c **** { 1718:srx1.c **** // Synch up with motion control 1719:srx1.c **** SynchWithMotion(); 4530 .LM799: 4531 165e 0E94 0000 call SynchWithMotion 1720:srx1.c **** 1721:srx1.c **** // If requested to, check for candle as we rotate 1722:srx1.c **** if (((RotateMessage*)pRotateCmd)->scan == TRUE) 4533 .LM800: 4534 1662 F801 movw r30,r16 4535 1664 F780 ldd r15,Z+7 4536 1666 F1E0 ldi r31,lo8(1) 4537 1668 FF16 cp r15,r31 4538 166a 69F4 brne .L287 1723:srx1.c **** { 1724:srx1.c **** if (bit_test(MOTION_SENSE) || (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE)) 4540 .LM801: 4541 166c 0B99 sbic 33-0x20,3 4542 166e 06C0 rjmp .L292 4543 1670 62E0 ldi r22,lo8(2) 4544 1672 80EB ldi r24,lo8(-80) 4545 1674 0E94 0000 call i2c_readbyte 4546 1678 8130 cpi r24,lo8(1) 4547 167a 29F4 brne .L287 4548 .L292: 1725:srx1.c **** { 1726:srx1.c **** ((RotateMessage*)pRotateCmd)->status = TRUE; 4550 .LM802: 4551 167c F801 movw r30,r16 4552 167e F086 std Z+8,r15 1727:srx1.c **** EmergencyStop(); // Slam the breaks hard! 4554 .LM803: 4555 1680 0E94 0000 call EmergencyStop 1728:srx1.c **** break; // exit loop prematurely. 4557 .LM804: 4558 1684 05C0 rjmp .L288 4559 .L287: 4560 1686 2197 sbiw r28,1 4561 1688 FFEF ldi r31,hi8(-1) 4562 168a CF3F cpi r28,lo8(-1) 4563 168c DF07 cpc r29,r31 4564 168e 39F7 brne .L293 4565 .L288: 1729:srx1.c **** } 1730:srx1.c **** } 1731:srx1.c **** } 1732:srx1.c **** 1733:srx1.c **** // Make sure we're totally stopped before starting to keep turn precision high 1734:srx1.c **** ArbitrateCmd.info.output_l = 0; 4567 .LM805: 4568 1690 1092 0000 sts (ArbitrateCmd.10+8)+1,__zero_reg__ 4569 1694 1092 0000 sts ArbitrateCmd.10+8,__zero_reg__ 1735:srx1.c **** ArbitrateCmd.info.output_r = 0; 4571 .LM806: 4572 1698 1092 0000 sts (ArbitrateCmd.10+10)+1,__zero_reg__ 4573 169c 1092 0000 sts ArbitrateCmd.10+10,__zero_reg__ 1736:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 4575 .LM807: 4576 16a0 60E0 ldi r22,lo8(ArbitrateCmd.10) 4577 16a2 70E0 ldi r23,hi8(ArbitrateCmd.10) 4578 16a4 80E0 ldi r24,lo8(ArbitrateQueue) 4579 16a6 90E0 ldi r25,hi8(ArbitrateQueue) 4580 16a8 0E94 0000 call AvrXSendMessage 1737:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 4582 .LM808: 4583 16ac 80E0 ldi r24,lo8(ArbitrateCmd.10) 4584 16ae 90E0 ldi r25,hi8(ArbitrateCmd.10) 4585 16b0 0E94 0000 call AvrXWaitMessageAck 1738:srx1.c **** WaitForMotionToStop(); 4587 .LM809: 4588 16b4 0E94 0000 call WaitForMotionToStop 1739:srx1.c **** 1740:srx1.c **** // We're done here. release. 1741:srx1.c **** ArbitrateCmd.info.active = FALSE; 4590 .LM810: 4591 16b8 1092 0000 sts ArbitrateCmd.10+6,__zero_reg__ 1742:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 4593 .LM811: 4594 16bc 60E0 ldi r22,lo8(ArbitrateCmd.10) 4595 16be 70E0 ldi r23,hi8(ArbitrateCmd.10) 4596 16c0 80E0 ldi r24,lo8(ArbitrateQueue) 4597 16c2 90E0 ldi r25,hi8(ArbitrateQueue) 4598 16c4 0E94 0000 call AvrXSendMessage 1743:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 4600 .LM812: 4601 16c8 80E0 ldi r24,lo8(ArbitrateCmd.10) 4602 16ca 90E0 ldi r25,hi8(ArbitrateCmd.10) 4603 16cc 0E94 0000 call AvrXWaitMessageAck 1744:srx1.c **** 1745:srx1.c **** // To prevent errors. 1746:srx1.c **** //WaitForMotionToStop(); 1747:srx1.c **** 1748:srx1.c **** // report back! 1749:srx1.c **** AvrXAckMessage(pRotateCmd); 4605 .LM813: 4606 16d0 C801 movw r24,r16 4607 16d2 0E94 0000 call AvrXAckMessage 4608 16d6 42CF rjmp .L294 1750:srx1.c **** } 1751:srx1.c **** } 4610 .LM814: 4611 .LBE6: 4612 /* epilogue: frame size=0 */ 4613 /* epilogue: naked */ 4614 /* epilogue end (size=0) */ 4615 /* function rotate size 198 (198) */ 4623 .Lscope5: 4625 .lcomm ArbitrateCmd.11,14 4627 .global follow 4629 follow: 1752:srx1.c **** 1753:srx1.c **** 1754:srx1.c **** 1755:srx1.c **** /****************************************** 1756:srx1.c **** Task - follow (wall following) 1757:srx1.c **** ******************************************/ 1758:srx1.c **** NAKEDFUNC(follow) 1759:srx1.c **** { 4631 .LM815: 4632 /* prologue: frame size=0 */ 4633 /* prologue: naked */ 4634 /* prologue end (size=0) */ 1760:srx1.c **** MessageControlBlock *pFollowCmd; 1761:srx1.c **** static ArbitrateMessage ArbitrateCmd; // static is required. 1762:srx1.c **** 1763:srx1.c **** unsigned char d_side_front; 1764:srx1.c **** unsigned char d_side_back; 1765:srx1.c **** unsigned char d_front; 1766:srx1.c **** signed int delta_speed; 1767:srx1.c **** signed int error; 1768:srx1.c **** signed int error_last; 1769:srx1.c **** unsigned char side; 1770:srx1.c **** unsigned char exit; 1771:srx1.c **** unsigned char line_sense_r, 1772:srx1.c **** line_sense_l; 1773:srx1.c **** 1774:srx1.c **** // Reduce code by initializing it once only. 1775:srx1.c **** ArbitrateCmd.id = FOLLOW; 4636 .LM816: 4637 .LBB7: 4638 16d8 81E0 ldi r24,lo8(1) 4639 16da 8093 0000 sts ArbitrateCmd.11+4,r24 1776:srx1.c **** ArbitrateCmd.info.type = ABSOLUTE; 4641 .LM817: 4642 16de 8093 0000 sts ArbitrateCmd.11+13,r24 4643 .L324: 1777:srx1.c **** 1778:srx1.c **** while (1) 1779:srx1.c **** { 1780:srx1.c **** pFollowCmd = AvrXWaitMessage(&FollowQueue); 4645 .LM818: 4646 16e2 80E0 ldi r24,lo8(FollowQueue) 4647 16e4 90E0 ldi r25,hi8(FollowQueue) 4648 16e6 0E94 0000 call AvrXWaitMessage 4649 16ea 8C01 movw r16,r24 1781:srx1.c **** 1782:srx1.c **** side = ((FollowMessage*)pFollowCmd)->side; 4651 .LM819: 4652 16ec FC01 movw r30,r24 4653 16ee 9480 ldd r9,Z+4 1783:srx1.c **** last_line_trigger = 0; 4655 .LM820: 4656 16f0 1092 0000 sts last_line_trigger,__zero_reg__ 1784:srx1.c **** 1785:srx1.c **** // Setup 1786:srx1.c **** error = 0; 4658 .LM821: 4659 16f4 CC24 clr r12 4660 16f6 DD24 clr r13 1787:srx1.c **** error_last = 0; 1788:srx1.c **** delta_speed = 0; 1789:srx1.c **** exit = FALSE; 4662 .LM822: 4663 16f8 AA24 clr r10 1790:srx1.c **** ArbitrateCmd.info.active = TRUE; 4665 .LM823: 4666 16fa 81E0 ldi r24,lo8(1) 4667 16fc 8093 0000 sts ArbitrateCmd.11+6,r24 4668 .L323: 1791:srx1.c **** 1792:srx1.c **** while (exit == FALSE) 1793:srx1.c **** { 1794:srx1.c **** // Synch up with motion control 1795:srx1.c **** SynchWithMotion(); 4670 .LM824: 4671 1700 0E94 0000 call SynchWithMotion 1796:srx1.c **** 1797:srx1.c **** // Select which side to work on 1798:srx1.c **** if (side == LEFT) 4673 .LM825: 4674 1704 9920 tst r9 4675 1706 31F4 brne .L303 1799:srx1.c **** { 1800:srx1.c **** d_side_front = gpd_read(GPD_L_F); 4677 .LM826: 4678 1708 81E0 ldi r24,lo8(1) 4679 170a 0E94 0000 call gpd_read 4680 170e F82E mov r15,r24 1801:srx1.c **** d_side_back = gpd_read(GPD_L_B); 4682 .LM827: 4683 1710 82E0 ldi r24,lo8(2) 4684 1712 05C0 rjmp .L327 4685 .L303: 1802:srx1.c **** } 1803:srx1.c **** else 1804:srx1.c **** { 1805:srx1.c **** d_side_front = gpd_read(GPD_R_F); 4687 .LM828: 4688 1714 83E0 ldi r24,lo8(3) 4689 1716 0E94 0000 call gpd_read 4690 171a F82E mov r15,r24 1806:srx1.c **** d_side_back = gpd_read(GPD_R_B); 4692 .LM829: 4693 171c 84E0 ldi r24,lo8(4) 4694 .L327: 4695 171e 0E94 0000 call gpd_read 4696 1722 C82F mov r28,r24 1807:srx1.c **** } 1808:srx1.c **** 1809:srx1.c **** d_front = gpd_read(GPD_F); 4698 .LM830: 4699 1724 80E0 ldi r24,lo8(0) 4700 1726 0E94 0000 call gpd_read 4701 172a B82E mov r11,r24 1810:srx1.c **** 1811:srx1.c **** // Debug information 1812:srx1.c **** if (follow_debug) 4703 .LM831: 4704 172c 8091 0000 lds r24,follow_debug 4705 1730 8823 tst r24 4706 1732 89F0 breq .L305 1813:srx1.c **** { 1814:srx1.c **** PutCRLF(); 4708 .LM832: 4709 1734 0E94 0000 call PutCRLF 1815:srx1.c **** PutDecByte(d_front); 4711 .LM833: 4712 1738 8B2D mov r24,r11 4713 173a 0E94 0000 call PutDecByte 1816:srx1.c **** PutChar('-'); 4715 .LM834: 4716 173e 8DE2 ldi r24,lo8(45) 4717 1740 0E94 0000 call PutChar 1817:srx1.c **** PutDecByte(d_side_front); 4719 .LM835: 4720 1744 8F2D mov r24,r15 4721 1746 0E94 0000 call PutDecByte 1818:srx1.c **** PutChar('-'); 4723 .LM836: 4724 174a 8DE2 ldi r24,lo8(45) 4725 174c 0E94 0000 call PutChar 1819:srx1.c **** PutDecByte(d_side_back); 4727 .LM837: 4728 1750 8C2F mov r24,r28 4729 1752 0E94 0000 call PutDecByte 4730 .L305: 1820:srx1.c **** } 1821:srx1.c **** 1822:srx1.c **** line_sense_l = adc_read(LINE_SENSOR_L); 4732 .LM838: 4733 1756 86E0 ldi r24,lo8(6) 4734 1758 0E94 0000 call adc_read 4735 175c E82E mov r14,r24 1823:srx1.c **** line_sense_r = adc_read(LINE_SENSOR_R); 4737 .LM839: 4738 175e 87E0 ldi r24,lo8(7) 4739 1760 0E94 0000 call adc_read 4740 1764 282F mov r18,r24 1824:srx1.c **** 1825:srx1.c **** if (d_front < FOLLOW_FRONT_THRESHOLD) // Obstacle in front? 4742 .LM840: 4743 1766 FFE0 ldi r31,lo8(15) 4744 1768 FB15 cp r31,r11 4745 176a 30F0 brlo .L306 1826:srx1.c **** { 1827:srx1.c **** ((FollowMessage*)pFollowCmd)->status = FOLLOW_REPORT_OBSTACLE; //status report. 4747 .LM841: 4748 176c 82E0 ldi r24,lo8(2) 4749 176e F801 movw r30,r16 4750 1770 8683 std Z+6,r24 1828:srx1.c **** exit = TRUE; 4752 .LM842: 4753 1772 F1E0 ldi r31,lo8(1) 4754 1774 AF2E mov r10,r31 4755 1776 8EC0 rjmp .L307 4756 .L306: 1829:srx1.c **** } 1830:srx1.c **** else // Did we pass a door frame? If yes, report back. 1831:srx1.c **** if ((((FollowMessage*)pFollowCmd)->ignore_door != TRUE) && ((LINE_SENSOR(line_sense_l) || LINE_S 4758 .LM843: 4759 1778 F801 movw r30,r16 4760 177a 8581 ldd r24,Z+5 4761 177c 8130 cpi r24,lo8(1) 4762 177e C1F0 breq .L308 4763 1780 9091 0000 lds r25,R_ParameterTable+6 4764 1784 9E15 cp r25,r14 4765 1786 10F0 brlo .L309 4766 1788 9217 cp r25,r18 4767 178a 90F4 brsh .L308 4768 .L309: 1832:srx1.c **** { 1833:srx1.c **** // Keep track which sensor fired first 1834:srx1.c **** if (LINE_SENSOR(line_sense_l)) 4770 .LM844: 4771 178c 9E15 cp r25,r14 4772 178e 18F4 brsh .L310 1835:srx1.c **** last_line_trigger = LINE_SENSOR_LEFT; 4774 .LM845: 4775 1790 81E0 ldi r24,lo8(1) 4776 1792 8093 0000 sts last_line_trigger,r24 4777 .L310: 1836:srx1.c **** 1837:srx1.c **** if (LINE_SENSOR(line_sense_r)) 4779 .LM846: 4780 1796 9217 cp r25,r18 4781 1798 28F4 brsh .L311 1838:srx1.c **** last_line_trigger += LINE_SENSOR_RIGHT; // Special case: if both sensors triggered simultaneou 4783 .LM847: 4784 179a 8091 0000 lds r24,last_line_trigger 4785 179e 8E5F subi r24,lo8(-(2)) 4786 17a0 8093 0000 sts last_line_trigger,r24 4787 .L311: 1839:srx1.c **** 1840:srx1.c **** exit = TRUE; 4789 .LM848: 4790 17a4 E1E0 ldi r30,lo8(1) 4791 17a6 AE2E mov r10,r30 1841:srx1.c **** ((FollowMessage*)pFollowCmd)->status = FOLLOW_REPORT_DOOR; //status report. 4793 .LM849: 4794 17a8 83E0 ldi r24,lo8(3) 4795 17aa F801 movw r30,r16 4796 17ac 8683 std Z+6,r24 4797 17ae 72C0 rjmp .L307 4798 .L308: 1842:srx1.c **** } 1843:srx1.c **** else // Otherwise, follow wall. 1844:srx1.c **** if ( d_side_front < FOLLOW_SIDE_THRESHOLD && d_side_back < FOLLOW_SIDE_THRESHOLD) 4800 .LM850: 4801 17b0 F8E1 ldi r31,lo8(24) 4802 17b2 FF15 cp r31,r15 4803 17b4 08F4 brsh .+2 4804 17b6 60C0 rjmp .L313 4805 17b8 FC17 cp r31,r28 4806 17ba 08F4 brsh .+2 4807 17bc 5DC0 rjmp .L313 1845:srx1.c **** { 1846:srx1.c **** error_last = error; 4809 .LM851: 4810 17be A601 movw r20,r12 1847:srx1.c **** error = (FOLLOW_DISTANCE_FACTOR*(FOLLOW_DISTANCE_SETPOINT - ((d_side_front + d_side_back) 4812 .LM852: 4813 17c0 2C2F mov r18,r28 4814 17c2 3327 clr r19 4815 17c4 8F2D mov r24,r15 4816 17c6 9927 clr r25 4817 17c8 6901 movw r12,r18 4818 17ca C81A sub r12,r24 4819 17cc D90A sbc r13,r25 4820 17ce CC0C add r12,r12 4821 17d0 DD1C adc r13,r13 4822 17d2 820F add r24,r18 4823 17d4 931F adc r25,r19 4824 17d6 9595 asr r25 4825 17d8 8795 ror r24 4826 17da C81A sub r12,r24 4827 17dc D90A sbc r13,r25 4828 17de 68E0 ldi r22,lo8(8) 4829 17e0 70E0 ldi r23,hi8(8) 4830 17e2 C60E add r12,r22 4831 17e4 D71E adc r13,r23 1848:srx1.c **** 1849:srx1.c **** //delta_speed = ((error * F_P) + ((error-error_last) * F_D))/F_O; 1850:srx1.c **** delta_speed = ((error * R_ParameterTable.f_p) + ((error-error_last) * R_ParameterTable.f_ 4833 .LM853: 4834 17e6 8091 0000 lds r24,R_ParameterTable+1 4835 17ea 9927 clr r25 4836 17ec C89E mul r12,r24 4837 17ee 9001 movw r18,r0 4838 17f0 C99E mul r12,r25 4839 17f2 300D add r19,r0 4840 17f4 D89E mul r13,r24 4841 17f6 300D add r19,r0 4842 17f8 1124 clr r1 4843 17fa C601 movw r24,r12 4844 17fc 841B sub r24,r20 4845 17fe 950B sbc r25,r21 4846 1800 AC01 movw r20,r24 4847 1802 8091 0000 lds r24,R_ParameterTable+2 4848 1806 9927 clr r25 4849 1808 489F mul r20,r24 4850 180a F001 movw r30,r0 4851 180c 499F mul r20,r25 4852 180e F00D add r31,r0 4853 1810 589F mul r21,r24 4854 1812 F00D add r31,r0 4855 1814 1124 clr r1 4856 1816 2E0F add r18,r30 4857 1818 3F1F adc r19,r31 4858 181a C901 movw r24,r18 4859 181c 6AE0 ldi r22,lo8(10) 4860 181e 70E0 ldi r23,hi8(10) 4861 1820 0E94 0000 call __divmodhi4 4862 1824 EB01 movw r28,r22 1851:srx1.c **** 1852:srx1.c **** #ifdef wall_follow_debug2 1853:srx1.c **** PutChar('='); 1854:srx1.c **** PutDecSignedWord((FOLLOW_DISTANCE_SETPOINT - ((d_side_front + d_side_back)/2))); 1855:srx1.c **** PutChar('+'); 1856:srx1.c **** PutDecSignedWord(FOLLOW_STEERING_FACTOR*(d_side_back-d_side_front)); 1857:srx1.c **** PutChar('='); 1858:srx1.c **** PutDecSignedWord(error); 1859:srx1.c **** #endif 1860:srx1.c **** 1861:srx1.c **** // Debug Information 1862:srx1.c **** if (follow_debug) 4864 .LM854: 4865 1826 8091 0000 lds r24,follow_debug 4866 182a 8823 tst r24 4867 182c 31F0 breq .L315 1863:srx1.c **** { 1864:srx1.c **** PutChar(':'); 4869 .LM855: 4870 182e 8AE3 ldi r24,lo8(58) 4871 1830 0E94 0000 call PutChar 1865:srx1.c **** PutDecSignedWord(delta_speed); 4873 .LM856: 4874 1834 CE01 movw r24,r28 4875 1836 0E94 0000 call PutDecSignedWord 4876 .L315: 1866:srx1.c **** } 1867:srx1.c **** 1868:srx1.c **** if (side == LEFT) 4878 .LM857: 4879 183a 9E01 movw r18,r28 4880 183c 2E5D subi r18,lo8(-(34)) 4881 183e 3F4F sbci r19,hi8(-(34)) 4882 1840 9920 tst r9 4883 1842 69F4 brne .L316 1869:srx1.c **** { 1870:srx1.c **** ArbitrateCmd.info.output_l = FOLLOW_FORWARD_SPEED + delta_speed; 4885 .LM858: 4886 1844 3093 0000 sts (ArbitrateCmd.11+8)+1,r19 4887 1848 2093 0000 sts ArbitrateCmd.11+8,r18 1871:srx1.c **** ArbitrateCmd.info.output_r = FOLLOW_FORWARD_SPEED + (-delta_speed); 4889 .LM859: 4890 184c 82E2 ldi r24,lo8(34) 4891 184e 90E0 ldi r25,hi8(34) 4892 1850 8C1B sub r24,r28 4893 1852 9D0B sbc r25,r29 4894 1854 9093 0000 sts (ArbitrateCmd.11+10)+1,r25 4895 1858 8093 0000 sts ArbitrateCmd.11+10,r24 4896 185c 1BC0 rjmp .L307 4897 .L316: 1872:srx1.c **** } 1873:srx1.c **** else 1874:srx1.c **** { 1875:srx1.c **** ArbitrateCmd.info.output_l = FOLLOW_FORWARD_SPEED + (-delta_speed); 4899 .LM860: 4900 185e 82E2 ldi r24,lo8(34) 4901 1860 90E0 ldi r25,hi8(34) 4902 1862 8C1B sub r24,r28 4903 1864 9D0B sbc r25,r29 4904 1866 9093 0000 sts (ArbitrateCmd.11+8)+1,r25 4905 186a 8093 0000 sts ArbitrateCmd.11+8,r24 1876:srx1.c **** ArbitrateCmd.info.output_r = FOLLOW_FORWARD_SPEED + delta_speed; 4907 .LM861: 4908 186e 3093 0000 sts (ArbitrateCmd.11+10)+1,r19 4909 1872 2093 0000 sts ArbitrateCmd.11+10,r18 4910 1876 0EC0 rjmp .L307 4911 .L313: 1877:srx1.c **** } 1878:srx1.c **** } 1879:srx1.c **** else // wall as ended so report back. 1880:srx1.c **** if ( d_side_front >= FOLLOW_SIDE_THRESHOLD && d_side_back < FOLLOW_SIDE_THRESHOLD) 4913 .LM862: 4914 1878 F8E1 ldi r31,lo8(24) 4915 187a FF15 cp r31,r15 4916 187c 38F4 brsh .L319 4917 187e FC17 cp r31,r28 4918 1880 28F0 brlo .L319 1881:srx1.c **** { 1882:srx1.c **** exit = TRUE; 4920 .LM863: 4921 1882 61E0 ldi r22,lo8(1) 4922 1884 A62E mov r10,r22 1883:srx1.c **** ((FollowMessage*)pFollowCmd)->status = FOLLOW_REPORT_SUCCESS; //status report. 4924 .LM864: 4925 1886 F801 movw r30,r16 4926 1888 A682 std Z+6,r10 4927 188a 04C0 rjmp .L307 4928 .L319: 1884:srx1.c **** } 1885:srx1.c **** else // Unexpected error. report back. 1886:srx1.c **** { 1887:srx1.c **** exit = TRUE; 4930 .LM865: 4931 188c 51E0 ldi r21,lo8(1) 4932 188e A52E mov r10,r21 1888:srx1.c **** ((FollowMessage*)pFollowCmd)->status = FOLLOW_REPORT_FAILURE; //status report. 4934 .LM866: 4935 1890 F801 movw r30,r16 4936 1892 1682 std Z+6,__zero_reg__ 4937 .L307: 1889:srx1.c **** } 1890:srx1.c **** 1891:srx1.c **** // Clean up if before exiting. 1892:srx1.c **** if (exit == TRUE) 4939 .LM867: 4940 1894 F1E0 ldi r31,lo8(1) 4941 1896 AF16 cp r10,r31 4942 1898 51F4 brne .L321 1893:srx1.c **** { 1894:srx1.c **** ArbitrateCmd.info.output_l = 0; 4944 .LM868: 4945 189a 1092 0000 sts (ArbitrateCmd.11+8)+1,__zero_reg__ 4946 189e 1092 0000 sts ArbitrateCmd.11+8,__zero_reg__ 1895:srx1.c **** ArbitrateCmd.info.output_r = 0; 4948 .LM869: 4949 18a2 1092 0000 sts (ArbitrateCmd.11+10)+1,__zero_reg__ 4950 18a6 1092 0000 sts ArbitrateCmd.11+10,__zero_reg__ 1896:srx1.c **** ArbitrateCmd.info.active = FALSE; 4952 .LM870: 4953 18aa 1092 0000 sts ArbitrateCmd.11+6,__zero_reg__ 4954 .L321: 1897:srx1.c **** } 1898:srx1.c **** 1899:srx1.c **** // Apply our decision! 1900:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 4956 .LM871: 4957 18ae 60E0 ldi r22,lo8(ArbitrateCmd.11) 4958 18b0 70E0 ldi r23,hi8(ArbitrateCmd.11) 4959 18b2 80E0 ldi r24,lo8(ArbitrateQueue) 4960 18b4 90E0 ldi r25,hi8(ArbitrateQueue) 4961 18b6 0E94 0000 call AvrXSendMessage 1901:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 4963 .LM872: 4964 18ba 80E0 ldi r24,lo8(ArbitrateCmd.11) 4965 18bc 90E0 ldi r25,hi8(ArbitrateCmd.11) 4966 18be 0E94 0000 call AvrXWaitMessageAck 1902:srx1.c **** 1903:srx1.c **** if (exit != TRUE) 4968 .LM873: 4969 18c2 61E0 ldi r22,lo8(1) 4970 18c4 A616 cp r10,r22 4971 18c6 31F0 breq .L300 1904:srx1.c **** AvrXDelay(&follow_timer, FOLLOW_LOOP_DELAY); // Give a chance to lower priority tasks to ru 4973 .LM874: 4974 18c8 62ED ldi r22,lo8(210) 4975 18ca 70E0 ldi r23,hi8(210) 4976 18cc 80E0 ldi r24,lo8(follow_timer) 4977 18ce 90E0 ldi r25,hi8(follow_timer) 4978 18d0 0E94 0000 call AvrXDelay 4979 .L300: 4980 18d4 AA20 tst r10 4981 18d6 09F4 brne .+2 4982 18d8 13CF rjmp .L323 1905:srx1.c **** } 1906:srx1.c **** 1907:srx1.c **** // We're done for now. Report back to requestor and await next command. 1908:srx1.c **** AvrXAckMessage(pFollowCmd); 4984 .LM875: 4985 18da C801 movw r24,r16 4986 18dc 0E94 0000 call AvrXAckMessage 4987 18e0 00CF rjmp .L324 1909:srx1.c **** } 1910:srx1.c **** } 4989 .LM876: 4990 .LBE7: 4991 /* epilogue: frame size=0 */ 4992 /* epilogue: naked */ 4993 /* epilogue end (size=0) */ 4994 /* function follow size 261 (261) */ 5010 .Lscope6: 5012 .lcomm ArbitrateCmd.12,14 5013 .lcomm ForwardCmd.13,8 5015 .global align 5017 align: 1911:srx1.c **** 1912:srx1.c **** /****************************************** 1913:srx1.c **** Task - align (Align with door frame or wall) 1914:srx1.c **** ******************************************/ 1915:srx1.c **** NAKEDFUNC(align) 1916:srx1.c **** { 5019 .LM877: 5020 /* prologue: frame size=0 */ 5021 /* prologue: naked */ 5022 /* prologue end (size=0) */ 1917:srx1.c **** MessageControlBlock *pAlignCmd; 1918:srx1.c **** static ArbitrateMessage ArbitrateCmd; // static is required. 1919:srx1.c **** static ForwardMessage ForwardCmd; 1920:srx1.c **** 1921:srx1.c **** unsigned char d_side_front; 1922:srx1.c **** unsigned char d_side_back; 1923:srx1.c **** signed int delta_speed; 1924:srx1.c **** signed int error; 1925:srx1.c **** signed int error_last; 1926:srx1.c **** unsigned char type; 1927:srx1.c **** unsigned char exit; 1928:srx1.c **** unsigned char line_sense_l, 1929:srx1.c **** line_sense_r; 1930:srx1.c **** 1931:srx1.c **** // Reduce code by initializing it once only. 1932:srx1.c **** ArbitrateCmd.id = ALIGN; 5024 .LM878: 5025 .LBB8: 5026 18e2 89E0 ldi r24,lo8(9) 5027 18e4 8093 0000 sts ArbitrateCmd.12+4,r24 1933:srx1.c **** ArbitrateCmd.info.type = ABSOLUTE; 5029 .LM879: 5030 18e8 81E0 ldi r24,lo8(1) 5031 18ea 8093 0000 sts ArbitrateCmd.12+13,r24 5032 .L368: 1934:srx1.c **** 1935:srx1.c **** while (1) 1936:srx1.c **** { 1937:srx1.c **** pAlignCmd = AvrXWaitMessage(&AlignQueue); 5034 .LM880: 5035 18ee 80E0 ldi r24,lo8(AlignQueue) 5036 18f0 90E0 ldi r25,hi8(AlignQueue) 5037 18f2 0E94 0000 call AvrXWaitMessage 5038 18f6 7C01 movw r14,r24 1938:srx1.c **** type = ((AlignMessage*)pAlignCmd)->type; 5040 .LM881: 5041 18f8 FC01 movw r30,r24 5042 18fa D480 ldd r13,Z+4 1939:srx1.c **** 1940:srx1.c **** error = 0; 5044 .LM882: 5045 18fc 00E0 ldi r16,lo8(0) 5046 18fe 10E0 ldi r17,hi8(0) 1941:srx1.c **** error_last = 0; 1942:srx1.c **** delta_speed = 0; 5048 .LM883: 5049 1900 E801 movw r28,r16 1943:srx1.c **** exit = FALSE; 5051 .LM884: 5052 1902 CC24 clr r12 1944:srx1.c **** ArbitrateCmd.info.active = TRUE; 5054 .LM885: 5055 1904 81E0 ldi r24,lo8(1) 5056 1906 8093 0000 sts ArbitrateCmd.12+6,r24 1945:srx1.c **** ArbitrateCmd.info.output_l = 0; 5058 .LM886: 5059 190a 1093 0000 sts (ArbitrateCmd.12+8)+1,r17 5060 190e 0093 0000 sts ArbitrateCmd.12+8,r16 1946:srx1.c **** ArbitrateCmd.info.output_r = 0; 5062 .LM887: 5063 1912 1093 0000 sts (ArbitrateCmd.12+10)+1,r17 5064 1916 0093 0000 sts ArbitrateCmd.12+10,r16 1947:srx1.c **** ((AlignMessage*)pAlignCmd)->status = ALIGN_REPORT_SUCCESS; 5066 .LM888: 5067 191a 8583 std Z+5,r24 1948:srx1.c **** 1949:srx1.c **** if (type == ALIGN_DOOR) // Align with door frame 5069 .LM889: 5070 191c F2E0 ldi r31,lo8(2) 5071 191e DF16 cp r13,r31 5072 1920 09F0 breq .+2 5073 1922 C1C0 rjmp .L367 1950:srx1.c **** { 1951:srx1.c **** // Update sensor readings 1952:srx1.c **** line_sense_l = adc_read(LINE_SENSOR_L); 5075 .LM890: 5076 1924 86E0 ldi r24,lo8(6) 5077 1926 0E94 0000 call adc_read 5078 192a 182F mov r17,r24 1953:srx1.c **** line_sense_r = adc_read(LINE_SENSOR_R); 5080 .LM891: 5081 192c 87E0 ldi r24,lo8(7) 5082 192e 0E94 0000 call adc_read 5083 1932 082F mov r16,r24 1954:srx1.c **** 1955:srx1.c **** // 1956:srx1.c **** // Left - Figure out where we're at and act accordingly 1957:srx1.c **** // 1958:srx1.c **** 1959:srx1.c **** // We triggered but went too far 1960:srx1.c **** if ((last_line_trigger == LINE_SENSOR_LEFT || last_line_trigger == LINE_SENSOR_BOTH) && !LINE_SE 5085 .LM892: 5086 1934 9091 0000 lds r25,last_line_trigger 5087 1938 9130 cpi r25,lo8(1) 5088 193a 11F0 breq .L334 5089 193c 9330 cpi r25,lo8(3) 5090 193e 79F4 brne .L333 5091 .L334: 5092 1940 8091 0000 lds r24,R_ParameterTable+6 5093 1944 8117 cp r24,r17 5094 1946 58F0 brlo .L333 1961:srx1.c **** { 1962:srx1.c **** // Backup until sensor in view. 1963:srx1.c **** ArbitrateCmd.info.output_l = -ALIGN_DOOR_SPEED_LO; 5096 .LM893: 5097 1948 82EF ldi r24,lo8(-14) 5098 194a 9FEF ldi r25,hi8(-14) 5099 194c 9093 0000 sts (ArbitrateCmd.12+8)+1,r25 5100 1950 8093 0000 sts ArbitrateCmd.12+8,r24 1964:srx1.c **** ArbitrateCmd.info.output_r = 0; 5102 .LM894: 5103 1954 D093 0000 sts (ArbitrateCmd.12+10)+1,r29 5104 1958 C093 0000 sts ArbitrateCmd.12+10,r28 1965:srx1.c **** 1966:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5106 .LM895: 5107 195c 10C0 rjmp .L371 5108 .L333: 1967:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 1968:srx1.c **** } 1969:srx1.c **** else // Didn't trigger at all. 1970:srx1.c **** if ((last_line_trigger != LINE_SENSOR_LEFT) && !LINE_SENSOR(line_sense_l)) 5110 .LM896: 5111 195e 9130 cpi r25,lo8(1) 5112 1960 C1F0 breq .L337 5113 1962 8091 0000 lds r24,R_ParameterTable+6 5114 1966 8117 cp r24,r17 5115 1968 A0F0 brlo .L337 1971:srx1.c **** { 1972:srx1.c **** // Stopped Short. Move forward till in view. 1973:srx1.c **** ArbitrateCmd.info.output_l = ALIGN_DOOR_SPEED_LO; 5117 .LM897: 5118 196a 8EE0 ldi r24,lo8(14) 5119 196c 90E0 ldi r25,hi8(14) 5120 196e 9093 0000 sts (ArbitrateCmd.12+8)+1,r25 5121 1972 8093 0000 sts ArbitrateCmd.12+8,r24 1974:srx1.c **** ArbitrateCmd.info.output_r = 0; 5123 .LM898: 5124 1976 1092 0000 sts (ArbitrateCmd.12+10)+1,__zero_reg__ 5125 197a 1092 0000 sts ArbitrateCmd.12+10,__zero_reg__ 5126 .L371: 1975:srx1.c **** 1976:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5128 .LM899: 5129 197e 60E0 ldi r22,lo8(ArbitrateCmd.12) 5130 1980 70E0 ldi r23,hi8(ArbitrateCmd.12) 5131 1982 80E0 ldi r24,lo8(ArbitrateQueue) 5132 1984 90E0 ldi r25,hi8(ArbitrateQueue) 5133 1986 0E94 0000 call AvrXSendMessage 1977:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5135 .LM900: 5136 198a 80E0 ldi r24,lo8(ArbitrateCmd.12) 5137 198c 90E0 ldi r25,hi8(ArbitrateCmd.12) 5138 198e 0E94 0000 call AvrXWaitMessageAck 5139 .L337: 1978:srx1.c **** } 1979:srx1.c **** 1980:srx1.c **** // Correct it! 1981:srx1.c **** do 1982:srx1.c **** { 1983:srx1.c **** // Synch up with motion control 1984:srx1.c **** SynchWithMotion(); 5141 .LM901: 5142 1992 0E94 0000 call SynchWithMotion 1985:srx1.c **** 1986:srx1.c **** line_sense_l = adc_read(LINE_SENSOR_L); 5144 .LM902: 5145 1996 86E0 ldi r24,lo8(6) 5146 1998 0E94 0000 call adc_read 5147 199c 182F mov r17,r24 1987:srx1.c **** } 5149 .LM903: 5150 199e 8091 0000 lds r24,R_ParameterTable+6 5151 19a2 8117 cp r24,r17 5152 19a4 B0F7 brsh .L337 1988:srx1.c **** while (!LINE_SENSOR(line_sense_l)); 1989:srx1.c **** 1990:srx1.c **** 1991:srx1.c **** 1992:srx1.c **** // We're done with Left side. 1993:srx1.c **** ArbitrateCmd.info.output_l = 0; 5154 .LM904: 5155 19a6 1092 0000 sts (ArbitrateCmd.12+8)+1,__zero_reg__ 5156 19aa 1092 0000 sts ArbitrateCmd.12+8,__zero_reg__ 1994:srx1.c **** ArbitrateCmd.info.output_r = 0; 5158 .LM905: 5159 19ae 1092 0000 sts (ArbitrateCmd.12+10)+1,__zero_reg__ 5160 19b2 1092 0000 sts ArbitrateCmd.12+10,__zero_reg__ 1995:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5162 .LM906: 5163 19b6 60E0 ldi r22,lo8(ArbitrateCmd.12) 5164 19b8 70E0 ldi r23,hi8(ArbitrateCmd.12) 5165 19ba 80E0 ldi r24,lo8(ArbitrateQueue) 5166 19bc 90E0 ldi r25,hi8(ArbitrateQueue) 5167 19be 0E94 0000 call AvrXSendMessage 1996:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5169 .LM907: 5170 19c2 80E0 ldi r24,lo8(ArbitrateCmd.12) 5171 19c4 90E0 ldi r25,hi8(ArbitrateCmd.12) 5172 19c6 0E94 0000 call AvrXWaitMessageAck 1997:srx1.c **** 1998:srx1.c **** // 1999:srx1.c **** // Right - Figure out where we're at and act accordingly 2000:srx1.c **** // 2001:srx1.c **** 2002:srx1.c **** // We triggered but went too far 2003:srx1.c **** if ((last_line_trigger == LINE_SENSOR_RIGHT || last_line_trigger == LINE_SENSOR_BOTH) && !LINE_S 5174 .LM908: 5175 19ca 9091 0000 lds r25,last_line_trigger 5176 19ce 892F mov r24,r25 5177 19d0 8250 subi r24,lo8(-(-2)) 5178 19d2 8230 cpi r24,lo8(2) 5179 19d4 58F4 brsh .L341 5180 19d6 8091 0000 lds r24,R_ParameterTable+6 5181 19da 8017 cp r24,r16 5182 19dc 38F0 brlo .L341 2004:srx1.c **** { 2005:srx1.c **** // Backup until sensor in view. 2006:srx1.c **** ArbitrateCmd.info.output_l = 0; 5184 .LM909: 5185 19de 1092 0000 sts (ArbitrateCmd.12+8)+1,__zero_reg__ 5186 19e2 1092 0000 sts ArbitrateCmd.12+8,__zero_reg__ 2007:srx1.c **** ArbitrateCmd.info.output_r = -ALIGN_DOOR_SPEED_LO; 5188 .LM910: 5189 19e6 82EF ldi r24,lo8(-14) 5190 19e8 9FEF ldi r25,hi8(-14) 5191 19ea 0CC0 rjmp .L372 5192 .L341: 2008:srx1.c **** 2009:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2010:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2011:srx1.c **** } 2012:srx1.c **** else // Didn't trigger at all. 2013:srx1.c **** if ((last_line_trigger != LINE_SENSOR_RIGHT) && !LINE_SENSOR(line_sense_r)) 5194 .LM911: 5195 19ec 9230 cpi r25,lo8(2) 5196 19ee C1F0 breq .L344 5197 19f0 8091 0000 lds r24,R_ParameterTable+6 5198 19f4 8017 cp r24,r16 5199 19f6 A0F0 brlo .L344 2014:srx1.c **** { 2015:srx1.c **** // Stopped Short. Move forward till in view. 2016:srx1.c **** ArbitrateCmd.info.output_l = 0; 5201 .LM912: 5202 19f8 1092 0000 sts (ArbitrateCmd.12+8)+1,__zero_reg__ 5203 19fc 1092 0000 sts ArbitrateCmd.12+8,__zero_reg__ 2017:srx1.c **** ArbitrateCmd.info.output_r = ALIGN_DOOR_SPEED_LO; 5205 .LM913: 5206 1a00 8EE0 ldi r24,lo8(14) 5207 1a02 90E0 ldi r25,hi8(14) 5208 .L372: 5209 1a04 9093 0000 sts (ArbitrateCmd.12+10)+1,r25 5210 1a08 8093 0000 sts ArbitrateCmd.12+10,r24 2018:srx1.c **** 2019:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5212 .LM914: 5213 1a0c 60E0 ldi r22,lo8(ArbitrateCmd.12) 5214 1a0e 70E0 ldi r23,hi8(ArbitrateCmd.12) 5215 1a10 80E0 ldi r24,lo8(ArbitrateQueue) 5216 1a12 90E0 ldi r25,hi8(ArbitrateQueue) 5217 1a14 0E94 0000 call AvrXSendMessage 2020:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5219 .LM915: 5220 1a18 80E0 ldi r24,lo8(ArbitrateCmd.12) 5221 1a1a 90E0 ldi r25,hi8(ArbitrateCmd.12) 5222 1a1c 0E94 0000 call AvrXWaitMessageAck 5223 .L344: 2021:srx1.c **** } 2022:srx1.c **** 2023:srx1.c **** // Correct it! 2024:srx1.c **** do 2025:srx1.c **** { 2026:srx1.c **** // Synch up with motion control 2027:srx1.c **** SynchWithMotion(); 5225 .LM916: 5226 1a20 0E94 0000 call SynchWithMotion 2028:srx1.c **** 2029:srx1.c **** line_sense_r = adc_read(LINE_SENSOR_R); 5228 .LM917: 5229 1a24 87E0 ldi r24,lo8(7) 5230 1a26 0E94 0000 call adc_read 5231 1a2a 082F mov r16,r24 2030:srx1.c **** } 5233 .LM918: 5234 1a2c 8091 0000 lds r24,R_ParameterTable+6 5235 1a30 8017 cp r24,r16 5236 1a32 B0F7 brsh .L344 2031:srx1.c **** while (!LINE_SENSOR(line_sense_r)); 2032:srx1.c **** 2033:srx1.c **** // We're done with Right side. 2034:srx1.c **** ArbitrateCmd.info.output_l = 0; 5238 .LM919: 5239 1a34 1092 0000 sts (ArbitrateCmd.12+8)+1,__zero_reg__ 5240 1a38 1092 0000 sts ArbitrateCmd.12+8,__zero_reg__ 2035:srx1.c **** ArbitrateCmd.info.output_r = 0; 5242 .LM920: 5243 1a3c 1092 0000 sts (ArbitrateCmd.12+10)+1,__zero_reg__ 5244 1a40 1092 0000 sts ArbitrateCmd.12+10,__zero_reg__ 2036:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5246 .LM921: 5247 1a44 60E0 ldi r22,lo8(ArbitrateCmd.12) 5248 1a46 70E0 ldi r23,hi8(ArbitrateCmd.12) 5249 1a48 80E0 ldi r24,lo8(ArbitrateQueue) 5250 1a4a 90E0 ldi r25,hi8(ArbitrateQueue) 5251 1a4c 0E94 0000 call AvrXSendMessage 2037:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5253 .LM922: 5254 1a50 80E0 ldi r24,lo8(ArbitrateCmd.12) 5255 1a52 90E0 ldi r25,hi8(ArbitrateCmd.12) 5256 1a54 0E94 0000 call AvrXWaitMessageAck 2038:srx1.c **** 2039:srx1.c **** // Clear line 2040:srx1.c **** WaitForMotionToStop(); 5258 .LM923: 5259 1a58 0E94 0000 call WaitForMotionToStop 2041:srx1.c **** ForwardCmd.lenght = ALIGN_CLEAR_DISTANCE; 5261 .LM924: 5262 1a5c 88E0 ldi r24,lo8(8) 5263 1a5e 90E0 ldi r25,hi8(8) 5264 1a60 9093 0000 sts (ForwardCmd.13+4)+1,r25 5265 1a64 8093 0000 sts ForwardCmd.13+4,r24 2042:srx1.c **** AvrXSendMessage(&ForwardQueue, &ForwardCmd.mcb); 5267 .LM925: 5268 1a68 60E0 ldi r22,lo8(ForwardCmd.13) 5269 1a6a 70E0 ldi r23,hi8(ForwardCmd.13) 5270 1a6c 80E0 ldi r24,lo8(ForwardQueue) 5271 1a6e 90E0 ldi r25,hi8(ForwardQueue) 5272 1a70 0E94 0000 call AvrXSendMessage 2043:srx1.c **** AvrXWaitMessageAck(&ForwardCmd.mcb); 5274 .LM926: 5275 1a74 80E0 ldi r24,lo8(ForwardCmd.13) 5276 1a76 90E0 ldi r25,hi8(ForwardCmd.13) 5277 1a78 0E94 0000 call AvrXWaitMessageAck 2044:srx1.c **** 2045:srx1.c **** // We're done here. 2046:srx1.c **** ArbitrateCmd.info.output_l = 0; 5279 .LM927: 5280 1a7c 1092 0000 sts (ArbitrateCmd.12+8)+1,__zero_reg__ 5281 1a80 1092 0000 sts ArbitrateCmd.12+8,__zero_reg__ 2047:srx1.c **** ArbitrateCmd.info.output_r = 0; 5283 .LM928: 5284 1a84 1092 0000 sts (ArbitrateCmd.12+10)+1,__zero_reg__ 5285 1a88 1092 0000 sts ArbitrateCmd.12+10,__zero_reg__ 2048:srx1.c **** ArbitrateCmd.info.active = FALSE; 5287 .LM929: 5288 1a8c 1092 0000 sts ArbitrateCmd.12+6,__zero_reg__ 2049:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5290 .LM930: 5291 1a90 60E0 ldi r22,lo8(ArbitrateCmd.12) 5292 1a92 70E0 ldi r23,hi8(ArbitrateCmd.12) 5293 1a94 80E0 ldi r24,lo8(ArbitrateQueue) 5294 1a96 90E0 ldi r25,hi8(ArbitrateQueue) 5295 1a98 0E94 0000 call AvrXSendMessage 2050:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5297 .LM931: 5298 1a9c 80E0 ldi r24,lo8(ArbitrateCmd.12) 5299 1a9e 90E0 ldi r25,hi8(ArbitrateCmd.12) 5300 1aa0 0E94 0000 call AvrXWaitMessageAck 5301 1aa4 84C0 rjmp .L348 5302 .L367: 2051:srx1.c **** } 2052:srx1.c **** else // Align with wall 2053:srx1.c **** { 2054:srx1.c **** while (exit == FALSE) 2055:srx1.c **** { 2056:srx1.c **** // Synch up with motion control 2057:srx1.c **** SynchWithMotion(); 5304 .LM932: 5305 1aa6 0E94 0000 call SynchWithMotion 2058:srx1.c **** 2059:srx1.c **** // Select which side to work on 2060:srx1.c **** if (type == ALIGN_LEFT) 5307 .LM933: 5308 1aaa DD20 tst r13 5309 1aac 31F4 brne .L352 2061:srx1.c **** { 2062:srx1.c **** d_side_front = gpd_read(GPD_L_F); 5311 .LM934: 5312 1aae 81E0 ldi r24,lo8(1) 5313 1ab0 0E94 0000 call gpd_read 5314 1ab4 C82F mov r28,r24 2063:srx1.c **** d_side_back = gpd_read(GPD_L_B); 5316 .LM935: 5317 1ab6 82E0 ldi r24,lo8(2) 5318 1ab8 05C0 rjmp .L373 5319 .L352: 2064:srx1.c **** } 2065:srx1.c **** else 2066:srx1.c **** { 2067:srx1.c **** d_side_front = gpd_read(GPD_R_F); 5321 .LM936: 5322 1aba 83E0 ldi r24,lo8(3) 5323 1abc 0E94 0000 call gpd_read 5324 1ac0 C82F mov r28,r24 2068:srx1.c **** d_side_back = gpd_read(GPD_R_B); 5326 .LM937: 5327 1ac2 84E0 ldi r24,lo8(4) 5328 .L373: 5329 1ac4 0E94 0000 call gpd_read 2069:srx1.c **** } 2070:srx1.c **** 2071:srx1.c **** if (d_side_back == d_side_front) // We're aligned 5331 .LM938: 5332 1ac8 8C17 cp r24,r28 5333 1aca 29F4 brne .L354 2072:srx1.c **** { 2073:srx1.c **** exit = TRUE; 5335 .LM939: 5336 1acc A1E0 ldi r26,lo8(1) 5337 1ace CA2E mov r12,r26 2074:srx1.c **** ((AlignMessage*)pAlignCmd)->status = ALIGN_REPORT_SUCCESS; //ALIGN_REPORT_SUCCESS 5339 .LM940: 5340 1ad0 F701 movw r30,r14 5341 1ad2 C582 std Z+5,r12 5342 1ad4 4AC0 rjmp .L355 5343 .L354: 2075:srx1.c **** } 2076:srx1.c **** else 2077:srx1.c **** if ((d_side_front >= ALIGN_SIDE_THRESHOLD) || (d_side_back >= ALIGN_SIDE_THRESHOLD) ) 5345 .LM941: 5346 1ad6 C931 cpi r28,lo8(25) 5347 1ad8 10F4 brsh .L357 5348 1ada 8931 cpi r24,lo8(25) 5349 1adc 60F0 brlo .L370 5350 .L357: 2078:srx1.c **** { 2079:srx1.c **** if (d_side_front >= ALIGN_SIDE_THRESHOLD) // Front sensor blind, move forward. 5352 .LM942: 5353 1ade C931 cpi r28,lo8(25) 5354 1ae0 18F0 brlo .L358 2080:srx1.c **** { 2081:srx1.c **** ArbitrateCmd.info.output_l = -ALIGN_CORRECTION_SPEED; 5356 .LM943: 5357 1ae2 83EF ldi r24,lo8(-13) 5358 1ae4 9FEF ldi r25,hi8(-13) 5359 1ae6 02C0 rjmp .L375 5360 .L358: 2082:srx1.c **** ArbitrateCmd.info.output_r = -ALIGN_CORRECTION_SPEED; 2083:srx1.c **** } 2084:srx1.c **** else // Rear sensor blind, move backwards 2085:srx1.c **** { 2086:srx1.c **** ArbitrateCmd.info.output_l = ALIGN_CORRECTION_SPEED; 5362 .LM944: 5363 1ae8 8DE0 ldi r24,lo8(13) 5364 1aea 90E0 ldi r25,hi8(13) 5365 .L375: 5366 1aec 9093 0000 sts (ArbitrateCmd.12+8)+1,r25 5367 1af0 8093 0000 sts ArbitrateCmd.12+8,r24 2087:srx1.c **** ArbitrateCmd.info.output_r = ALIGN_CORRECTION_SPEED; 5369 .LM945: 5370 1af4 2DC0 rjmp .L374 2088:srx1.c **** } 2089:srx1.c **** } 2090:srx1.c **** else // Align ourselves 2091:srx1.c **** if ((d_side_front < ALIGN_SIDE_THRESHOLD) && (d_side_back < ALIGN_SIDE_THRESHOLD) ) 2092:srx1.c **** { 2093:srx1.c **** error_last = error; 5372 .LM946: 5373 .L370: 5374 1af6 A801 movw r20,r16 2094:srx1.c **** error = (ALIGN_STEERING_FACTOR *(d_side_back-d_side_front)); 5376 .LM947: 5377 1af8 082F mov r16,r24 5378 1afa 1127 clr r17 5379 1afc 0C1B sub r16,r28 5380 1afe 1109 sbc r17,__zero_reg__ 2095:srx1.c **** 2096:srx1.c **** // TODO: Use seperate coefficient for Align and Follow??? 2097:srx1.c **** delta_speed = ((error * R_ParameterTable.f_p) + ((error-error_last) * R_ParameterTabl 5382 .LM948: 5383 1b00 8091 0000 lds r24,R_ParameterTable+1 5384 1b04 9927 clr r25 5385 1b06 089F mul r16,r24 5386 1b08 9001 movw r18,r0 5387 1b0a 099F mul r16,r25 5388 1b0c 300D add r19,r0 5389 1b0e 189F mul r17,r24 5390 1b10 300D add r19,r0 5391 1b12 1124 clr r1 5392 1b14 B801 movw r22,r16 5393 1b16 641B sub r22,r20 5394 1b18 750B sbc r23,r21 5395 1b1a 8091 0000 lds r24,R_ParameterTable+2 5396 1b1e 9927 clr r25 5397 1b20 689F mul r22,r24 5398 1b22 F001 movw r30,r0 5399 1b24 699F mul r22,r25 5400 1b26 F00D add r31,r0 5401 1b28 789F mul r23,r24 5402 1b2a F00D add r31,r0 5403 1b2c 1124 clr r1 5404 1b2e 2E0F add r18,r30 5405 1b30 3F1F adc r19,r31 5406 1b32 C901 movw r24,r18 5407 1b34 6AE0 ldi r22,lo8(10) 5408 1b36 70E0 ldi r23,hi8(10) 5409 1b38 0E94 0000 call __divmodhi4 2098:srx1.c **** 2099:srx1.c **** if (type == ALIGN_LEFT) 5411 .LM949: 5412 1b3c 8827 clr r24 5413 1b3e 9927 clr r25 5414 1b40 861B sub r24,r22 5415 1b42 970B sbc r25,r23 5416 1b44 DD20 tst r13 5417 1b46 49F4 brne .L362 2100:srx1.c **** { 2101:srx1.c **** ArbitrateCmd.info.output_l = delta_speed; 5419 .LM950: 5420 1b48 7093 0000 sts (ArbitrateCmd.12+8)+1,r23 5421 1b4c 6093 0000 sts ArbitrateCmd.12+8,r22 5422 .L374: 2102:srx1.c **** ArbitrateCmd.info.output_r = -delta_speed; 5424 .LM951: 5425 1b50 9093 0000 sts (ArbitrateCmd.12+10)+1,r25 5426 1b54 8093 0000 sts ArbitrateCmd.12+10,r24 5427 1b58 08C0 rjmp .L355 5428 .L362: 2103:srx1.c **** } 2104:srx1.c **** else 2105:srx1.c **** { 2106:srx1.c **** ArbitrateCmd.info.output_l = -delta_speed; 5430 .LM952: 5431 1b5a 9093 0000 sts (ArbitrateCmd.12+8)+1,r25 5432 1b5e 8093 0000 sts ArbitrateCmd.12+8,r24 2107:srx1.c **** ArbitrateCmd.info.output_r = delta_speed; 5434 .LM953: 5435 1b62 7093 0000 sts (ArbitrateCmd.12+10)+1,r23 5436 1b66 6093 0000 sts ArbitrateCmd.12+10,r22 5437 .L355: 2108:srx1.c **** } 2109:srx1.c **** } 2110:srx1.c **** else // We can't align, report back the failure. 2111:srx1.c **** { 2112:srx1.c **** ((AlignMessage*)pAlignCmd)->status = ALIGN_REPORT_FAILURE; // Error. 2113:srx1.c **** exit = TRUE; 2114:srx1.c **** } 2115:srx1.c **** 2116:srx1.c **** // Clean up on exit 2117:srx1.c **** if (exit == TRUE) 5439 .LM954: 5440 1b6a F1E0 ldi r31,lo8(1) 5441 1b6c CF16 cp r12,r31 5442 1b6e 51F4 brne .L365 2118:srx1.c **** { 2119:srx1.c **** ArbitrateCmd.info.output_l = 0; 5444 .LM955: 5445 1b70 1092 0000 sts (ArbitrateCmd.12+8)+1,__zero_reg__ 5446 1b74 1092 0000 sts ArbitrateCmd.12+8,__zero_reg__ 2120:srx1.c **** ArbitrateCmd.info.output_r = 0; 5448 .LM956: 5449 1b78 1092 0000 sts (ArbitrateCmd.12+10)+1,__zero_reg__ 5450 1b7c 1092 0000 sts ArbitrateCmd.12+10,__zero_reg__ 2121:srx1.c **** ArbitrateCmd.info.active = FALSE; 5452 .LM957: 5453 1b80 1092 0000 sts ArbitrateCmd.12+6,__zero_reg__ 5454 .L365: 2122:srx1.c **** } 2123:srx1.c **** 2124:srx1.c **** // Apply our decision! 2125:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5456 .LM958: 5457 1b84 60E0 ldi r22,lo8(ArbitrateCmd.12) 5458 1b86 70E0 ldi r23,hi8(ArbitrateCmd.12) 5459 1b88 80E0 ldi r24,lo8(ArbitrateQueue) 5460 1b8a 90E0 ldi r25,hi8(ArbitrateQueue) 5461 1b8c 0E94 0000 call AvrXSendMessage 2126:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5463 .LM959: 5464 1b90 80E0 ldi r24,lo8(ArbitrateCmd.12) 5465 1b92 90E0 ldi r25,hi8(ArbitrateCmd.12) 5466 1b94 0E94 0000 call AvrXWaitMessageAck 2127:srx1.c **** 2128:srx1.c **** if (exit != FALSE) 5468 .LM960: 5469 1b98 CC20 tst r12 5470 1b9a 31F0 breq .L349 2129:srx1.c **** AvrXDelay(&align_timer, ALIGN_LOOP_DELAY); // Give a chance to lower priority tasks to 5472 .LM961: 5473 1b9c 62ED ldi r22,lo8(210) 5474 1b9e 70E0 ldi r23,hi8(210) 5475 1ba0 80E0 ldi r24,lo8(align_timer) 5476 1ba2 90E0 ldi r25,hi8(align_timer) 5477 1ba4 0E94 0000 call AvrXDelay 5478 .L349: 5479 1ba8 CC20 tst r12 5480 1baa 09F4 brne .+2 5481 1bac 7CCF rjmp .L367 5482 .L348: 2130:srx1.c **** } 2131:srx1.c **** } 2132:srx1.c **** 2133:srx1.c **** // We're done for now. Report back to requestor and await next command. 2134:srx1.c **** AvrXAckMessage(pAlignCmd); 5484 .LM962: 5485 1bae C701 movw r24,r14 5486 1bb0 0E94 0000 call AvrXAckMessage 5487 1bb4 9CCE rjmp .L368 2135:srx1.c **** } 2136:srx1.c **** } 5489 .LM963: 5490 .LBE8: 5491 /* epilogue: frame size=0 */ 5492 /* epilogue: naked */ 5493 /* epilogue end (size=0) */ 5494 /* function align size 362 (362) */ 5510 .Lscope7: 5512 .lcomm ArbitrateCmd.14,14 5513 .lcomm RotateCmd.15,9 5515 .global goat 5517 goat: 2137:srx1.c **** 2138:srx1.c **** 2139:srx1.c **** /****************************************** 2140:srx1.c **** Task - goat 2141:srx1.c **** ******************************************/ 2142:srx1.c **** NAKEDFUNC(goat) 2143:srx1.c **** { 5519 .LM964: 5520 /* prologue: frame size=0 */ 5521 /* prologue: naked */ 5522 /* prologue end (size=0) */ 2144:srx1.c **** MessageControlBlock *pGoAtCmd; 2145:srx1.c **** static ArbitrateMessage ArbitrateCmd; 2146:srx1.c **** static RotateMessage RotateCmd; 2147:srx1.c **** 2148:srx1.c **** int target_x, target_y; 2149:srx1.c **** int target_bearing; 2150:srx1.c **** unsigned char exit; 2151:srx1.c **** int delta_x; 2152:srx1.c **** int delta_y; 2153:srx1.c **** int distance; 2154:srx1.c **** int bearing; 2155:srx1.c **** int error; 2156:srx1.c **** int delta_theta; 2157:srx1.c **** int sign, iTurn; 2158:srx1.c **** 2159:srx1.c **** // Reduce code by initializing it once only. 2160:srx1.c **** ArbitrateCmd.id = GOAT; 5524 .LM965: 5525 .LBB9: 5526 1bb6 84E0 ldi r24,lo8(4) 5527 1bb8 8093 0000 sts ArbitrateCmd.14+4,r24 2161:srx1.c **** ArbitrateCmd.info.type = ABSOLUTE; 5529 .LM966: 5530 1bbc 81E0 ldi r24,lo8(1) 5531 1bbe 8093 0000 sts ArbitrateCmd.14+13,r24 2162:srx1.c **** ArbitrateCmd.info.active = TRUE; 5533 .LM967: 5534 1bc2 8093 0000 sts ArbitrateCmd.14+6,r24 5535 .L390: 2163:srx1.c **** 2164:srx1.c **** //#define GOAT_DEBUG 1 2165:srx1.c **** 2166:srx1.c **** while (1) 2167:srx1.c **** { 2168:srx1.c **** exit = FALSE; 5537 .LM968: 5538 1bc6 3324 clr r3 2169:srx1.c **** 2170:srx1.c **** pGoAtCmd = AvrXWaitMessage(&GoAtQueue); 5540 .LM969: 5541 1bc8 80E0 ldi r24,lo8(GoAtQueue) 5542 1bca 90E0 ldi r25,hi8(GoAtQueue) 5543 1bcc 0E94 0000 call AvrXWaitMessage 5544 1bd0 5C01 movw r10,r24 2171:srx1.c **** 2172:srx1.c **** target_x = ((GoAtMessage*)pGoAtCmd)->x; 5546 .LM970: 5547 1bd2 FC01 movw r30,r24 5548 1bd4 6480 ldd r6,Z+4 5549 1bd6 7580 ldd r7,Z+5 2173:srx1.c **** target_y = ((GoAtMessage*)pGoAtCmd)->y; 5551 .LM971: 5552 1bd8 8680 ldd r8,Z+6 5553 1bda 9780 ldd r9,Z+7 2174:srx1.c **** target_bearing = ((GoAtMessage*)pGoAtCmd)->theta; 5555 .LM972: 5556 1bdc 4084 ldd r4,Z+8 5557 1bde 5184 ldd r5,Z+9 2175:srx1.c **** 2176:srx1.c **** // Point robot in right direction 2177:srx1.c **** delta_x = target_x - iX; 5559 .LM973: 5560 1be0 8091 0000 lds r24,iX 5561 1be4 9091 0000 lds r25,(iX)+1 5562 1be8 E301 movw r28,r6 5563 1bea C81B sub r28,r24 5564 1bec D90B sbc r29,r25 2178:srx1.c **** delta_y = target_y - iY; 5566 .LM974: 5567 1bee 8091 0000 lds r24,iY 5568 1bf2 9091 0000 lds r25,(iY)+1 5569 1bf6 6401 movw r12,r8 5570 1bf8 C81A sub r12,r24 5571 1bfa D90A sbc r13,r25 2179:srx1.c **** bearing = -atan2(delta_y, delta_x) * RAD2DEG; 5573 .LM975: 5574 1bfc CE01 movw r24,r28 5575 1bfe AA27 clr r26 5576 1c00 97FD sbrc r25,7 5577 1c02 A095 com r26 5578 1c04 BA2F mov r27,r26 5579 1c06 BC01 movw r22,r24 5580 1c08 CD01 movw r24,r26 5581 1c0a 0E94 0000 call __floatsisf 5582 1c0e 7B01 movw r14,r22 5583 1c10 8C01 movw r16,r24 5584 1c12 C601 movw r24,r12 5585 1c14 AA27 clr r26 5586 1c16 97FD sbrc r25,7 5587 1c18 A095 com r26 5588 1c1a BA2F mov r27,r26 5589 1c1c BC01 movw r22,r24 5590 1c1e CD01 movw r24,r26 5591 1c20 0E94 0000 call __floatsisf 5592 1c24 DC01 movw r26,r24 5593 1c26 CB01 movw r24,r22 5594 1c28 A801 movw r20,r16 5595 1c2a 9701 movw r18,r14 5596 1c2c BC01 movw r22,r24 5597 1c2e CD01 movw r24,r26 5598 1c30 0E94 0000 call atan2 5599 1c34 DC01 movw r26,r24 5600 1c36 CB01 movw r24,r22 5601 1c38 B058 subi r27,0x80 5602 1c3a 20EE ldi r18,lo8(0x42652ee0) 5603 1c3c 3EE2 ldi r19,hi8(0x42652ee0) 5604 1c3e 45E6 ldi r20,hlo8(0x42652ee0) 5605 1c40 52E4 ldi r21,hhi8(0x42652ee0) 5606 1c42 BC01 movw r22,r24 5607 1c44 CD01 movw r24,r26 5608 1c46 0E94 0000 call __mulsf3 5609 1c4a DC01 movw r26,r24 5610 1c4c CB01 movw r24,r22 5611 1c4e BC01 movw r22,r24 5612 1c50 CD01 movw r24,r26 5613 1c52 0E94 0000 call __fixsfsi 5614 1c56 DC01 movw r26,r24 5615 1c58 CB01 movw r24,r22 2180:srx1.c **** 2181:srx1.c **** #ifdef GOAT_DEBUG 2182:srx1.c **** PutChar('['); 2183:srx1.c **** PutDecSignedWord(target_x); 2184:srx1.c **** PutChar(','); 2185:srx1.c **** PutDecSignedWord(target_y); 2186:srx1.c **** PutChar(':'); 2187:srx1.c **** PutDecSignedWord(target_bearing); 2188:srx1.c **** PutChar('='); 2189:srx1.c **** PutDecSignedWord(bearing); 2190:srx1.c **** PutChar(']'); 2191:srx1.c **** #endif 2192:srx1.c **** 2193:srx1.c **** RotateCmd.angle = bearing; 5617 .LM976: 5618 1c5a 9093 0000 sts (RotateCmd.15+4)+1,r25 5619 1c5e 8093 0000 sts RotateCmd.15+4,r24 2194:srx1.c **** RotateCmd.type = ABSOLUTE; 5621 .LM977: 5622 1c62 81E0 ldi r24,lo8(1) 5623 1c64 8093 0000 sts RotateCmd.15+6,r24 2195:srx1.c **** AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); 5625 .LM978: 5626 1c68 60E0 ldi r22,lo8(RotateCmd.15) 5627 1c6a 70E0 ldi r23,hi8(RotateCmd.15) 5628 1c6c 80E0 ldi r24,lo8(RotateQueue) 5629 1c6e 90E0 ldi r25,hi8(RotateQueue) 5630 1c70 0E94 0000 call AvrXSendMessage 2196:srx1.c **** AvrXWaitMessageAck(&RotateCmd.mcb); 5632 .LM979: 5633 1c74 80E0 ldi r24,lo8(RotateCmd.15) 5634 1c76 90E0 ldi r25,hi8(RotateCmd.15) 5635 1c78 0E94 0000 call AvrXWaitMessageAck 5636 .L389: 2197:srx1.c **** 2198:srx1.c **** // TODO: Make forward speed inversely proportial to the distance to target. (slow down on appr 2199:srx1.c **** // TODO: Use a simple PD controller to have bearing dynamically adjusted to cooperate with AVOID. 2200:srx1.c **** 2201:srx1.c **** while (exit != TRUE) 2202:srx1.c **** { 2203:srx1.c **** 2204:srx1.c **** #ifdef GOAT_DEBUG 2205:srx1.c **** PutCRLF(); 2206:srx1.c **** PutChar(':'); 2207:srx1.c **** PutDecSignedWord(iX); 2208:srx1.c **** PutChar(':'); 2209:srx1.c **** PutDecSignedWord(iY); 2210:srx1.c **** #endif 2211:srx1.c **** SynchWithMotion(); 5638 .LM980: 5639 1c7c 0E94 0000 call SynchWithMotion 2212:srx1.c **** 2213:srx1.c **** delta_x = target_x - iX; 5641 .LM981: 5642 1c80 8091 0000 lds r24,iX 5643 1c84 9091 0000 lds r25,(iX)+1 5644 1c88 E301 movw r28,r6 5645 1c8a C81B sub r28,r24 5646 1c8c D90B sbc r29,r25 2214:srx1.c **** delta_y = target_y - iY; 5648 .LM982: 5649 1c8e 8091 0000 lds r24,iY 5650 1c92 9091 0000 lds r25,(iY)+1 5651 1c96 6401 movw r12,r8 5652 1c98 C81A sub r12,r24 5653 1c9a D90A sbc r13,r25 2215:srx1.c **** distance = sqrt(square(delta_x) + square(delta_y)); 5655 .LM983: 5656 1c9c CE01 movw r24,r28 5657 1c9e AA27 clr r26 5658 1ca0 97FD sbrc r25,7 5659 1ca2 A095 com r26 5660 1ca4 BA2F mov r27,r26 5661 1ca6 BC01 movw r22,r24 5662 1ca8 CD01 movw r24,r26 5663 1caa 0E94 0000 call __floatsisf 5664 1cae 7B01 movw r14,r22 5665 1cb0 8C01 movw r16,r24 5666 1cb2 C601 movw r24,r12 5667 1cb4 AA27 clr r26 5668 1cb6 97FD sbrc r25,7 5669 1cb8 A095 com r26 5670 1cba BA2F mov r27,r26 5671 1cbc BC01 movw r22,r24 5672 1cbe CD01 movw r24,r26 5673 1cc0 0E94 0000 call __floatsisf 5674 1cc4 DC01 movw r26,r24 5675 1cc6 CB01 movw r24,r22 2216:srx1.c **** bearing = -atan2(delta_y, delta_x) * RAD2DEG; 5677 .LM984: 5678 1cc8 A801 movw r20,r16 5679 1cca 9701 movw r18,r14 5680 1ccc BC01 movw r22,r24 5681 1cce CD01 movw r24,r26 5682 1cd0 0E94 0000 call atan2 5683 1cd4 DC01 movw r26,r24 5684 1cd6 CB01 movw r24,r22 5685 1cd8 B058 subi r27,0x80 5686 1cda 20EE ldi r18,lo8(0x42652ee0) 5687 1cdc 3EE2 ldi r19,hi8(0x42652ee0) 5688 1cde 45E6 ldi r20,hlo8(0x42652ee0) 5689 1ce0 52E4 ldi r21,hhi8(0x42652ee0) 5690 1ce2 BC01 movw r22,r24 5691 1ce4 CD01 movw r24,r26 5692 1ce6 0E94 0000 call __mulsf3 5693 1cea DC01 movw r26,r24 5694 1cec CB01 movw r24,r22 5695 1cee BC01 movw r22,r24 5696 1cf0 CD01 movw r24,r26 5697 1cf2 0E94 0000 call __fixsfsi 5698 1cf6 DC01 movw r26,r24 5699 1cf8 CB01 movw r24,r22 5700 1cfa 9C01 movw r18,r24 2217:srx1.c **** 2218:srx1.c **** if (abs(delta_x) < GOAT_DISTANCE_THRESHOLD && abs(delta_y) < GOAT_DISTANCE_THRESHOLD) 5702 .LM985: 5703 1cfc 2296 adiw r28,2 5704 1cfe 2597 sbiw r28,5 5705 1d00 58F4 brsh .L383 5706 1d02 82E0 ldi r24,lo8(2) 5707 1d04 90E0 ldi r25,hi8(2) 5708 1d06 C80E add r12,r24 5709 1d08 D91E adc r13,r25 5710 1d0a 95E0 ldi r25,lo8(5) 5711 1d0c C916 cp r12,r25 5712 1d0e D104 cpc r13,__zero_reg__ 5713 1d10 18F4 brsh .L383 2219:srx1.c **** { 2220:srx1.c **** exit = TRUE; 5715 .LM986: 5716 1d12 B1E0 ldi r27,lo8(1) 5717 1d14 3B2E mov r3,r27 5718 1d16 54C0 rjmp .L380 5719 .L383: 2221:srx1.c **** } 2222:srx1.c **** else 2223:srx1.c **** { 2224:srx1.c **** 2225:srx1.c **** // Normalize input (force within valid 0-359 range) 2226:srx1.c **** iTurn = bearing - iTheta; 5721 .LM987: 5722 1d18 8091 0000 lds r24,iTheta 5723 1d1c 9091 0000 lds r25,(iTheta)+1 5724 1d20 281B sub r18,r24 5725 1d22 390B sbc r19,r25 5726 1d24 C901 movw r24,r18 2227:srx1.c **** sign = (iTurn > 0) ? 1 : -1; 5728 .LM988: 5729 1d26 1216 cp __zero_reg__,r18 5730 1d28 1306 cpc __zero_reg__,r19 5731 1d2a 1CF4 brge .L385 5732 1d2c E1E0 ldi r30,lo8(1) 5733 1d2e F0E0 ldi r31,hi8(1) 5734 1d30 02C0 rjmp .L386 5735 .L385: 5736 1d32 EFEF ldi r30,lo8(-1) 5737 1d34 FFEF ldi r31,hi8(-1) 5738 .L386: 2228:srx1.c **** 2229:srx1.c **** iTurn = abs(iTurn) % 360; 5740 .LM989: 5741 1d36 9C01 movw r18,r24 5742 1d38 97FF sbrs r25,7 5743 1d3a 04C0 rjmp .L387 5744 1d3c 2227 clr r18 5745 1d3e 3327 clr r19 5746 1d40 281B sub r18,r24 5747 1d42 390B sbc r19,r25 5748 .L387: 5749 1d44 C8E6 ldi r28,lo8(360) 5750 1d46 D1E0 ldi r29,hi8(360) 5751 1d48 C901 movw r24,r18 5752 1d4a BE01 movw r22,r28 5753 1d4c 0E94 0000 call __divmodhi4 2230:srx1.c **** 2231:srx1.c **** // pick shortest route around the circle 2232:srx1.c **** if (iTurn > 180) 5755 .LM990: 5756 1d50 853B cpi r24,181 5757 1d52 9105 cpc r25,__zero_reg__ 5758 1d54 34F0 brlt .L388 2233:srx1.c **** { 2234:srx1.c **** iTurn = 360 - iTurn; 5760 .LM991: 5761 1d56 C81B sub r28,r24 5762 1d58 D90B sbc r29,r25 5763 1d5a CE01 movw r24,r28 2235:srx1.c **** sign = -sign; 5765 .LM992: 5766 1d5c F095 com r31 5767 1d5e E195 neg r30 5768 1d60 FF4F sbci r31,lo8(-1) 5769 .L388: 2236:srx1.c **** } 2237:srx1.c **** 2238:srx1.c **** delta_theta = iTurn * sign; 5771 .LM993: 5772 1d62 8E9F mul r24,r30 5773 1d64 9001 movw r18,r0 5774 1d66 8F9F mul r24,r31 5775 1d68 300D add r19,r0 5776 1d6a 9E9F mul r25,r30 5777 1d6c 300D add r19,r0 5778 1d6e 1124 clr r1 5779 1d70 C901 movw r24,r18 2239:srx1.c **** 2240:srx1.c **** //delta_theta = iTurn; //(((360+bearing)%360)-iTheta); 2241:srx1.c **** error = delta_theta * 2 / 10; // calculate bearing error 5781 .LM994: 5782 1d72 820F add r24,r18 5783 1d74 931F adc r25,r19 5784 1d76 6AE0 ldi r22,lo8(10) 5785 1d78 70E0 ldi r23,hi8(10) 5786 1d7a 0E94 0000 call __divmodhi4 5787 1d7e 9B01 movw r18,r22 2242:srx1.c **** 2243:srx1.c **** #ifdef GOAT_DEBUG 2244:srx1.c **** PutCRLF(); 2245:srx1.c **** PutDecSignedWord(iTheta); 2246:srx1.c **** PutChar(':'); 2247:srx1.c **** PutDecSignedWord(bearing); 2248:srx1.c **** PutChar(':'); 2249:srx1.c **** PutDecSignedWord(delta_theta); 2250:srx1.c **** PutChar('='); 2251:srx1.c **** PutDecSignedWord(error); 2252:srx1.c **** #endif 2253:srx1.c **** 2254:srx1.c **** // And correct... 2255:srx1.c **** ArbitrateCmd.info.output_l = POWER(50) + (error); 5789 .LM995: 5790 1d80 2C5E subi r18,lo8(-(20)) 5791 1d82 3F4F sbci r19,hi8(-(20)) 5792 1d84 3093 0000 sts (ArbitrateCmd.14+8)+1,r19 5793 1d88 2093 0000 sts ArbitrateCmd.14+8,r18 5794 1d8c 2451 subi r18,lo8(-(-20)) 5795 1d8e 3040 sbci r19,hi8(-(-20)) 2256:srx1.c **** ArbitrateCmd.info.output_r = POWER(50) + (-error); 5797 .LM996: 5798 1d90 84E1 ldi r24,lo8(20) 5799 1d92 90E0 ldi r25,hi8(20) 5800 1d94 821B sub r24,r18 5801 1d96 930B sbc r25,r19 5802 1d98 9093 0000 sts (ArbitrateCmd.14+10)+1,r25 5803 1d9c 8093 0000 sts ArbitrateCmd.14+10,r24 2257:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5805 .LM997: 5806 1da0 60E0 ldi r22,lo8(ArbitrateCmd.14) 5807 1da2 70E0 ldi r23,hi8(ArbitrateCmd.14) 5808 1da4 80E0 ldi r24,lo8(ArbitrateQueue) 5809 1da6 90E0 ldi r25,hi8(ArbitrateQueue) 5810 1da8 0E94 0000 call AvrXSendMessage 2258:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5812 .LM998: 5813 1dac 80E0 ldi r24,lo8(ArbitrateCmd.14) 5814 1dae 90E0 ldi r25,hi8(ArbitrateCmd.14) 5815 1db0 0E94 0000 call AvrXWaitMessageAck 2259:srx1.c **** 2260:srx1.c **** #ifdef GOAT_DEBUG2 2261:srx1.c **** PutCRLF(); 2262:srx1.c **** PutChar(':'); 2263:srx1.c **** PutDecSignedWord(delta_x); 2264:srx1.c **** PutChar(':'); 2265:srx1.c **** PutDecSignedWord(delta_y); 2266:srx1.c **** PutChar(':'); 2267:srx1.c **** PutDecSignedWord(distance); 2268:srx1.c **** #endif 2269:srx1.c **** 2270:srx1.c **** AvrXDelay(&goat_timer, GOAT_LOOP_DELAY); // Give a chance to lower priority tasks to ru 5817 .LM999: 5818 1db4 64EB ldi r22,lo8(180) 5819 1db6 70E0 ldi r23,hi8(180) 5820 1db8 80E0 ldi r24,lo8(goat_timer) 5821 1dba 90E0 ldi r25,hi8(goat_timer) 5822 1dbc 0E94 0000 call AvrXDelay 5823 .L380: 5824 1dc0 31E0 ldi r19,lo8(1) 5825 1dc2 3316 cp r3,r19 5826 1dc4 09F0 breq .+2 5827 1dc6 5ACF rjmp .L389 2271:srx1.c **** } 2272:srx1.c **** } 2273:srx1.c **** 2274:srx1.c **** // Stop here! 2275:srx1.c **** ArbitrateCmd.info.output_l = 0; 5829 .LM1000: 5830 1dc8 1092 0000 sts (ArbitrateCmd.14+8)+1,__zero_reg__ 5831 1dcc 1092 0000 sts ArbitrateCmd.14+8,__zero_reg__ 2276:srx1.c **** ArbitrateCmd.info.output_r = 0; 5833 .LM1001: 5834 1dd0 1092 0000 sts (ArbitrateCmd.14+10)+1,__zero_reg__ 5835 1dd4 1092 0000 sts ArbitrateCmd.14+10,__zero_reg__ 2277:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5837 .LM1002: 5838 1dd8 60E0 ldi r22,lo8(ArbitrateCmd.14) 5839 1dda 70E0 ldi r23,hi8(ArbitrateCmd.14) 5840 1ddc 80E0 ldi r24,lo8(ArbitrateQueue) 5841 1dde 90E0 ldi r25,hi8(ArbitrateQueue) 5842 1de0 0E94 0000 call AvrXSendMessage 2278:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5844 .LM1003: 5845 1de4 80E0 ldi r24,lo8(ArbitrateCmd.14) 5846 1de6 90E0 ldi r25,hi8(ArbitrateCmd.14) 5847 1de8 0E94 0000 call AvrXWaitMessageAck 2279:srx1.c **** 2280:srx1.c **** // Point robot in final direction 2281:srx1.c **** RotateCmd.angle = target_bearing; 5849 .LM1004: 5850 1dec 5092 0000 sts (RotateCmd.15+4)+1,r5 5851 1df0 4092 0000 sts RotateCmd.15+4,r4 2282:srx1.c **** RotateCmd.type = ABSOLUTE; 5853 .LM1005: 5854 1df4 3092 0000 sts RotateCmd.15+6,r3 2283:srx1.c **** AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); 5856 .LM1006: 5857 1df8 60E0 ldi r22,lo8(RotateCmd.15) 5858 1dfa 70E0 ldi r23,hi8(RotateCmd.15) 5859 1dfc 80E0 ldi r24,lo8(RotateQueue) 5860 1dfe 90E0 ldi r25,hi8(RotateQueue) 5861 1e00 0E94 0000 call AvrXSendMessage 2284:srx1.c **** AvrXWaitMessageAck(&RotateCmd.mcb); 5863 .LM1007: 5864 1e04 80E0 ldi r24,lo8(RotateCmd.15) 5865 1e06 90E0 ldi r25,hi8(RotateCmd.15) 5866 1e08 0E94 0000 call AvrXWaitMessageAck 2285:srx1.c **** 2286:srx1.c **** // Stop here! 2287:srx1.c **** ArbitrateCmd.info.output_l = 0; 5868 .LM1008: 5869 1e0c 1092 0000 sts (ArbitrateCmd.14+8)+1,__zero_reg__ 5870 1e10 1092 0000 sts ArbitrateCmd.14+8,__zero_reg__ 2288:srx1.c **** ArbitrateCmd.info.output_r = 0; 5872 .LM1009: 5873 1e14 1092 0000 sts (ArbitrateCmd.14+10)+1,__zero_reg__ 5874 1e18 1092 0000 sts ArbitrateCmd.14+10,__zero_reg__ 2289:srx1.c **** ArbitrateCmd.info.active = FALSE; 5876 .LM1010: 5877 1e1c 1092 0000 sts ArbitrateCmd.14+6,__zero_reg__ 2290:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5879 .LM1011: 5880 1e20 60E0 ldi r22,lo8(ArbitrateCmd.14) 5881 1e22 70E0 ldi r23,hi8(ArbitrateCmd.14) 5882 1e24 80E0 ldi r24,lo8(ArbitrateQueue) 5883 1e26 90E0 ldi r25,hi8(ArbitrateQueue) 5884 1e28 0E94 0000 call AvrXSendMessage 2291:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5886 .LM1012: 5887 1e2c 80E0 ldi r24,lo8(ArbitrateCmd.14) 5888 1e2e 90E0 ldi r25,hi8(ArbitrateCmd.14) 5889 1e30 0E94 0000 call AvrXWaitMessageAck 2292:srx1.c **** 2293:srx1.c **** AvrXAckMessage(pGoAtCmd); 5891 .LM1013: 5892 1e34 C501 movw r24,r10 5893 1e36 0E94 0000 call AvrXAckMessage 5894 1e3a C5CE rjmp .L390 2294:srx1.c **** } 2295:srx1.c **** } 5896 .LM1014: 5897 .LBE9: 5898 /* epilogue: frame size=0 */ 5899 /* epilogue: naked */ 5900 /* epilogue end (size=0) */ 5901 /* function goat size 324 (324) */ 5918 .Lscope8: 5920 .lcomm ArbitrateCmd.16,14 5921 .lcomm RotateCmd.17,9 5923 .global extinguish 5925 extinguish: 2296:srx1.c **** 2297:srx1.c **** /****************************************** 2298:srx1.c **** Task - extinguish 2299:srx1.c **** ******************************************/ 2300:srx1.c **** NAKEDFUNC(extinguish) 2301:srx1.c **** { 5927 .LM1015: 5928 /* prologue: frame size=0 */ 5929 /* prologue: naked */ 5930 /* prologue end (size=0) */ 2302:srx1.c **** MessageControlBlock *pExtinguishCmd; 2303:srx1.c **** static ArbitrateMessage ArbitrateCmd; // static is required. 2304:srx1.c **** static RotateMessage RotateCmd; 2305:srx1.c **** unsigned char position; 2306:srx1.c **** signed char error; 2307:srx1.c **** unsigned char line_sense_l, 2308:srx1.c **** line_sense_r; 2309:srx1.c **** unsigned char count; 2310:srx1.c **** 2311:srx1.c **** // Reduce code by initializing it once only. 2312:srx1.c **** ArbitrateCmd.id=EXTINGUISH; 5932 .LM1016: 5933 .LBB10: 5934 1e3c 87E0 ldi r24,lo8(7) 5935 1e3e 8093 0000 sts ArbitrateCmd.16+4,r24 2313:srx1.c **** ArbitrateCmd.info.type = ABSOLUTE; 5937 .LM1017: 5938 1e42 81E0 ldi r24,lo8(1) 5939 1e44 8093 0000 sts ArbitrateCmd.16+13,r24 5940 .L423: 2314:srx1.c **** 2315:srx1.c **** //#define EXTINGUISH_DEBUG 1 2316:srx1.c **** 2317:srx1.c **** while (1) 2318:srx1.c **** { 2319:srx1.c **** pExtinguishCmd = AvrXWaitMessage(&ExtinguishQueue); 5942 .LM1018: 5943 1e48 80E0 ldi r24,lo8(ExtinguishQueue) 5944 1e4a 90E0 ldi r25,hi8(ExtinguishQueue) 5945 1e4c 0E94 0000 call AvrXWaitMessage 5946 1e50 7C01 movw r14,r24 2320:srx1.c **** 2321:srx1.c **** ArbitrateCmd.info.active = TRUE; 5948 .LM1019: 5949 1e52 81E0 ldi r24,lo8(1) 5950 1e54 8093 0000 sts ArbitrateCmd.16+6,r24 2322:srx1.c **** 2323:srx1.c **** // TODO: Add a way to bail out if SCM can't find candle (darn flaky Motion sensor!)? 2324:srx1.c **** // Ex.: Start a timer and wait for it to expire. 2325:srx1.c **** // Keep turning Right until we've got the candle in sight 2326:srx1.c **** if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == FALSE) 5952 .LM1020: 5953 1e58 62E0 ldi r22,lo8(2) 5954 1e5a 80EB ldi r24,lo8(-80) 5955 1e5c 0E94 0000 call i2c_readbyte 5956 1e60 8823 tst r24 5957 1e62 09F5 brne .L397 2327:srx1.c **** { 2328:srx1.c **** ArbitrateCmd.info.output_l = EXTINGUISH_SCAN_SPEED; 5959 .LM1021: 5960 1e64 8BE0 ldi r24,lo8(11) 5961 1e66 90E0 ldi r25,hi8(11) 5962 1e68 9093 0000 sts (ArbitrateCmd.16+8)+1,r25 5963 1e6c 8093 0000 sts ArbitrateCmd.16+8,r24 2329:srx1.c **** ArbitrateCmd.info.output_r = -EXTINGUISH_SCAN_SPEED; 5965 .LM1022: 5966 1e70 85EF ldi r24,lo8(-11) 5967 1e72 9FEF ldi r25,hi8(-11) 5968 1e74 9093 0000 sts (ArbitrateCmd.16+10)+1,r25 5969 1e78 8093 0000 sts ArbitrateCmd.16+10,r24 2330:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 5971 .LM1023: 5972 1e7c 60E0 ldi r22,lo8(ArbitrateCmd.16) 5973 1e7e 70E0 ldi r23,hi8(ArbitrateCmd.16) 5974 1e80 80E0 ldi r24,lo8(ArbitrateQueue) 5975 1e82 90E0 ldi r25,hi8(ArbitrateQueue) 5976 1e84 0E94 0000 call AvrXSendMessage 2331:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 5978 .LM1024: 5979 1e88 80E0 ldi r24,lo8(ArbitrateCmd.16) 5980 1e8a 90E0 ldi r25,hi8(ArbitrateCmd.16) 5981 1e8c 0E94 0000 call AvrXWaitMessageAck 2332:srx1.c **** 2333:srx1.c **** while (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == FALSE) 5983 .LM1025: 5984 1e90 02C0 rjmp .L398 5985 .L401: 2334:srx1.c **** { 2335:srx1.c **** SynchWithMotion(); 5987 .LM1026: 5988 1e92 0E94 0000 call SynchWithMotion 5989 .L398: 5990 1e96 62E0 ldi r22,lo8(2) 5991 1e98 80EB ldi r24,lo8(-80) 5992 1e9a 0E94 0000 call i2c_readbyte 5993 1e9e 8823 tst r24 5994 1ea0 C1F3 breq .L401 2336:srx1.c **** } 2337:srx1.c **** 2338:srx1.c **** // "emergency" break (because we can't afford to miss the candle with all the latency) 2339:srx1.c **** EmergencyStop(); 5996 .LM1027: 5997 1ea2 0E94 0000 call EmergencyStop 5998 .L397: 2340:srx1.c **** } 2341:srx1.c **** 2342:srx1.c **** position = i2c_readbyte(SCM_I2C_ADDR, SCM_X); 6000 .LM1028: 6001 1ea6 64E0 ldi r22,lo8(4) 6002 1ea8 80EB ldi r24,lo8(-80) 6003 1eaa 0E94 0000 call i2c_readbyte 2343:srx1.c **** 2344:srx1.c **** // Refresh line sensors reading 2345:srx1.c **** line_sense_l = adc_read(LINE_SENSOR_L); 6005 .LM1029: 6006 1eae 86E0 ldi r24,lo8(6) 6007 1eb0 0E94 0000 call adc_read 2346:srx1.c **** line_sense_r = adc_read(LINE_SENSOR_R); 6009 .LM1030: 6010 1eb4 87E0 ldi r24,lo8(7) 6011 1eb6 0E94 0000 call adc_read 2347:srx1.c **** 2348:srx1.c **** count = EXTINGUISH_APPROACH_COUNT; 6013 .LM1031: 6014 1eba CDE0 ldi r28,lo8(13) 6015 .L409: 2349:srx1.c **** 2350:srx1.c **** // Proceed forward towards candle until white line is detected 2351:srx1.c **** while (count) 2352:srx1.c **** { 2353:srx1.c **** SynchWithMotion(); 6017 .LM1032: 6018 1ebc 0E94 0000 call SynchWithMotion 2354:srx1.c **** 2355:srx1.c **** // Refresh line sensors reading 2356:srx1.c **** line_sense_l = adc_read(LINE_SENSOR_L); 6020 .LM1033: 6021 1ec0 86E0 ldi r24,lo8(6) 6022 1ec2 0E94 0000 call adc_read 6023 1ec6 182F mov r17,r24 2357:srx1.c **** line_sense_r = adc_read(LINE_SENSOR_R); 6025 .LM1034: 6026 1ec8 87E0 ldi r24,lo8(7) 6027 1eca 0E94 0000 call adc_read 2358:srx1.c **** 2359:srx1.c **** // Keep moving forward for a few more paces within white circle. 2360:srx1.c **** if (LINE_SENSOR(line_sense_l) || LINE_SENSOR(line_sense_r)) 6029 .LM1035: 6030 1ece 9091 0000 lds r25,R_ParameterTable+6 6031 1ed2 9117 cp r25,r17 6032 1ed4 10F0 brlo .L406 6033 1ed6 9817 cp r25,r24 6034 1ed8 08F4 brsh .L405 6035 .L406: 2361:srx1.c **** { 2362:srx1.c **** count--; 6037 .LM1036: 6038 1eda C150 subi r28,lo8(-(-1)) 6039 .L405: 2363:srx1.c **** } 2364:srx1.c **** 2365:srx1.c **** if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS)) 6041 .LM1037: 6042 1edc 62E0 ldi r22,lo8(2) 6043 1ede 80EB ldi r24,lo8(-80) 6044 1ee0 0E94 0000 call i2c_readbyte 6045 1ee4 8823 tst r24 6046 1ee6 61F1 breq .L407 2366:srx1.c **** { 2367:srx1.c **** position = i2c_readbyte(SCM_I2C_ADDR, SCM_X); 6048 .LM1038: 6049 1ee8 64E0 ldi r22,lo8(4) 6050 1eea 80EB ldi r24,lo8(-80) 6051 1eec 0E94 0000 call i2c_readbyte 6052 1ef0 282F mov r18,r24 2368:srx1.c **** error = (64-position) * E_P / E_O; 6054 .LM1039: 6055 1ef2 80E4 ldi r24,lo8(64) 6056 1ef4 90E0 ldi r25,hi8(64) 6057 1ef6 821B sub r24,r18 6058 1ef8 9109 sbc r25,__zero_reg__ 6059 1efa 880F add r24,r24 6060 1efc 991F adc r25,r25 6061 1efe 6AE0 ldi r22,lo8(10) 6062 1f00 70E0 ldi r23,hi8(10) 6063 1f02 0E94 0000 call __divmodhi4 6064 1f06 CB01 movw r24,r22 6065 1f08 162F mov r17,r22 2369:srx1.c **** 2370:srx1.c **** PutDecSignedByte(error); 6067 .LM1040: 6068 1f0a 0E94 0000 call PutDecSignedByte 2371:srx1.c **** PutChar('-'); 6070 .LM1041: 6071 1f0e 8DE2 ldi r24,lo8(45) 6072 1f10 0E94 0000 call PutChar 2372:srx1.c **** 2373:srx1.c **** ArbitrateCmd.info.active = TRUE; 6074 .LM1042: 6075 1f14 81E0 ldi r24,lo8(1) 6076 1f16 8093 0000 sts ArbitrateCmd.16+6,r24 2374:srx1.c **** ArbitrateCmd.info.output_l = EXTINGUISH_FORWARD_SPEED + (-error); 6078 .LM1043: 6079 1f1a 212F mov r18,r17 6080 1f1c 3327 clr r19 6081 1f1e 27FD sbrc r18,7 6082 1f20 3095 com r19 6083 1f22 80E1 ldi r24,lo8(16) 6084 1f24 90E0 ldi r25,hi8(16) 6085 1f26 821B sub r24,r18 6086 1f28 930B sbc r25,r19 6087 1f2a 9093 0000 sts (ArbitrateCmd.16+8)+1,r25 6088 1f2e 8093 0000 sts ArbitrateCmd.16+8,r24 2375:srx1.c **** ArbitrateCmd.info.output_r = EXTINGUISH_FORWARD_SPEED + (error); 6090 .LM1044: 6091 1f32 205F subi r18,lo8(-(16)) 6092 1f34 3F4F sbci r19,hi8(-(16)) 6093 1f36 3093 0000 sts (ArbitrateCmd.16+10)+1,r19 6094 1f3a 2093 0000 sts ArbitrateCmd.16+10,r18 2376:srx1.c **** 2377:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 6096 .LM1045: 6097 1f3e 0AC0 rjmp .L427 6098 .L407: 2378:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2379:srx1.c **** } 2380:srx1.c **** else // if we lose target keep going forward. 2381:srx1.c **** { 2382:srx1.c **** // TODO: Change for a turn in the opposite dir. of last recorded turn?? 2383:srx1.c **** 2384:srx1.c **** ArbitrateCmd.info.output_l = EXTINGUISH_FORWARD_SPEED; 6100 .LM1046: 6101 1f40 80E1 ldi r24,lo8(16) 6102 1f42 90E0 ldi r25,hi8(16) 6103 1f44 9093 0000 sts (ArbitrateCmd.16+8)+1,r25 6104 1f48 8093 0000 sts ArbitrateCmd.16+8,r24 2385:srx1.c **** ArbitrateCmd.info.output_r = EXTINGUISH_FORWARD_SPEED; 6106 .LM1047: 6107 1f4c 9093 0000 sts (ArbitrateCmd.16+10)+1,r25 6108 1f50 8093 0000 sts ArbitrateCmd.16+10,r24 6109 .L427: 2386:srx1.c **** 2387:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 6111 .LM1048: 6112 1f54 60E0 ldi r22,lo8(ArbitrateCmd.16) 6113 1f56 70E0 ldi r23,hi8(ArbitrateCmd.16) 6114 1f58 80E0 ldi r24,lo8(ArbitrateQueue) 6115 1f5a 90E0 ldi r25,hi8(ArbitrateQueue) 6116 1f5c 0E94 0000 call AvrXSendMessage 2388:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 6118 .LM1049: 6119 1f60 80E0 ldi r24,lo8(ArbitrateCmd.16) 6120 1f62 90E0 ldi r25,hi8(ArbitrateCmd.16) 6121 1f64 0E94 0000 call AvrXWaitMessageAck 6122 1f68 CC23 tst r28 6123 1f6a 09F0 breq .+2 6124 1f6c A7CF rjmp .L409 2389:srx1.c **** } 2390:srx1.c **** } 2391:srx1.c **** 2392:srx1.c **** EmergencyStop(); 6126 .LM1050: 6127 1f6e 0E94 0000 call EmergencyStop 2393:srx1.c **** 2394:srx1.c **** // Stop, we're almost here! 2395:srx1.c **** ArbitrateCmd.info.output_l = POWER(0); // Stop motion 6129 .LM1051: 6130 1f72 1092 0000 sts (ArbitrateCmd.16+8)+1,__zero_reg__ 6131 1f76 1092 0000 sts ArbitrateCmd.16+8,__zero_reg__ 2396:srx1.c **** ArbitrateCmd.info.output_r = POWER(0); 6133 .LM1052: 6134 1f7a 1092 0000 sts (ArbitrateCmd.16+10)+1,__zero_reg__ 6135 1f7e 1092 0000 sts ArbitrateCmd.16+10,__zero_reg__ 2397:srx1.c **** //ArbitrateCmd.info.active = TRUE; 2398:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 6137 .LM1053: 6138 1f82 60E0 ldi r22,lo8(ArbitrateCmd.16) 6139 1f84 70E0 ldi r23,hi8(ArbitrateCmd.16) 6140 1f86 80E0 ldi r24,lo8(ArbitrateQueue) 6141 1f88 90E0 ldi r25,hi8(ArbitrateQueue) 6142 1f8a 0E94 0000 call AvrXSendMessage 2399:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 6144 .LM1054: 6145 1f8e 80E0 ldi r24,lo8(ArbitrateCmd.16) 6146 1f90 90E0 ldi r25,hi8(ArbitrateCmd.16) 6147 1f92 0E94 0000 call AvrXWaitMessageAck 2400:srx1.c **** 2401:srx1.c **** // Recalibrate SCM 2402:srx1.c **** SCM_calibrate(FALSE); // recalibrate without saving to EEPROM 6149 .LM1055: 6150 1f96 8C2F mov r24,r28 6151 1f98 0E94 0000 call SCM_calibrate 2403:srx1.c **** 2404:srx1.c **** position = i2c_readbyte(SCM_I2C_ADDR, SCM_X); 6153 .LM1056: 6154 1f9c 64E0 ldi r22,lo8(4) 6155 1f9e 80EB ldi r24,lo8(-80) 6156 1fa0 0E94 0000 call i2c_readbyte 6157 1fa4 282F mov r18,r24 2405:srx1.c **** error = (64-position) * E_P / E_O; 6159 .LM1057: 6160 1fa6 80E4 ldi r24,lo8(64) 6161 1fa8 90E0 ldi r25,hi8(64) 6162 1faa 821B sub r24,r18 6163 1fac 9109 sbc r25,__zero_reg__ 6164 1fae 880F add r24,r24 6165 1fb0 991F adc r25,r25 6166 1fb2 6AE0 ldi r22,lo8(10) 6167 1fb4 70E0 ldi r23,hi8(10) 6168 1fb6 0E94 0000 call __divmodhi4 6169 1fba 162F mov r17,r22 2406:srx1.c **** 2407:srx1.c **** // TODO: Add a timeout? 2408:srx1.c **** while (abs(error) > EXTINGUISH_THRESHOLD_ERROR) 6171 .LM1058: 6172 1fbc 42C0 rjmp .L426 6173 .L416: 2409:srx1.c **** { 2410:srx1.c **** SynchWithMotion(); 6175 .LM1059: 6176 1fbe 0E94 0000 call SynchWithMotion 2411:srx1.c **** 2412:srx1.c **** if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS)) 6178 .LM1060: 6179 1fc2 62E0 ldi r22,lo8(2) 6180 1fc4 80EB ldi r24,lo8(-80) 6181 1fc6 0E94 0000 call i2c_readbyte 6182 1fca 8823 tst r24 6183 1fcc 21F1 breq .L414 2413:srx1.c **** { 2414:srx1.c **** position = i2c_readbyte(SCM_I2C_ADDR, SCM_X); 6185 .LM1061: 6186 1fce 64E0 ldi r22,lo8(4) 6187 1fd0 80EB ldi r24,lo8(-80) 6188 1fd2 0E94 0000 call i2c_readbyte 6189 1fd6 282F mov r18,r24 2415:srx1.c **** error = (64-position) * E_P / E_O; 6191 .LM1062: 6192 1fd8 80E4 ldi r24,lo8(64) 6193 1fda 90E0 ldi r25,hi8(64) 6194 1fdc 821B sub r24,r18 6195 1fde 9109 sbc r25,__zero_reg__ 6196 1fe0 880F add r24,r24 6197 1fe2 991F adc r25,r25 6198 1fe4 6AE0 ldi r22,lo8(10) 6199 1fe6 70E0 ldi r23,hi8(10) 6200 1fe8 0E94 0000 call __divmodhi4 6201 1fec 162F mov r17,r22 2416:srx1.c **** 2417:srx1.c **** #ifdef EXTINGUISH_DEBUG 2418:srx1.c **** PutDecSignedByte(error); 2419:srx1.c **** PutChar('-'); 2420:srx1.c **** #endif 2421:srx1.c **** 2422:srx1.c **** ArbitrateCmd.info.active = TRUE; 6203 .LM1063: 6204 1fee 81E0 ldi r24,lo8(1) 6205 1ff0 8093 0000 sts ArbitrateCmd.16+6,r24 2423:srx1.c **** ArbitrateCmd.info.output_l = (-error); 6207 .LM1064: 6208 1ff4 262F mov r18,r22 6209 1ff6 3327 clr r19 6210 1ff8 27FD sbrc r18,7 6211 1ffa 3095 com r19 6212 1ffc 8827 clr r24 6213 1ffe 9927 clr r25 6214 2000 821B sub r24,r18 6215 2002 930B sbc r25,r19 6216 2004 9093 0000 sts (ArbitrateCmd.16+8)+1,r25 6217 2008 8093 0000 sts ArbitrateCmd.16+8,r24 2424:srx1.c **** ArbitrateCmd.info.output_r = (error); 6219 .LM1065: 6220 200c 3093 0000 sts (ArbitrateCmd.16+10)+1,r19 6221 2010 2093 0000 sts ArbitrateCmd.16+10,r18 2425:srx1.c **** 2426:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 6223 .LM1066: 6224 2014 0CC0 rjmp .L428 6225 .L414: 2427:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2428:srx1.c **** } 2429:srx1.c **** else // if we lose target keep Turning 2430:srx1.c **** { 2431:srx1.c **** ArbitrateCmd.info.output_l = -EXTINGUISH_SCAN_SPEED; 6227 .LM1067: 6228 2016 85EF ldi r24,lo8(-11) 6229 2018 9FEF ldi r25,hi8(-11) 6230 201a 9093 0000 sts (ArbitrateCmd.16+8)+1,r25 6231 201e 8093 0000 sts ArbitrateCmd.16+8,r24 2432:srx1.c **** ArbitrateCmd.info.output_r = EXTINGUISH_SCAN_SPEED; 6233 .LM1068: 6234 2022 8BE0 ldi r24,lo8(11) 6235 2024 90E0 ldi r25,hi8(11) 6236 2026 9093 0000 sts (ArbitrateCmd.16+10)+1,r25 6237 202a 8093 0000 sts ArbitrateCmd.16+10,r24 6238 .L428: 2433:srx1.c **** 2434:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 6240 .LM1069: 6241 202e 60E0 ldi r22,lo8(ArbitrateCmd.16) 6242 2030 70E0 ldi r23,hi8(ArbitrateCmd.16) 6243 2032 80E0 ldi r24,lo8(ArbitrateQueue) 6244 2034 90E0 ldi r25,hi8(ArbitrateQueue) 6245 2036 0E94 0000 call AvrXSendMessage 2435:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 6247 .LM1070: 6248 203a 80E0 ldi r24,lo8(ArbitrateCmd.16) 6249 203c 90E0 ldi r25,hi8(ArbitrateCmd.16) 6250 203e 0E94 0000 call AvrXWaitMessageAck 6251 .L426: 6252 2042 812F mov r24,r17 6253 2044 9927 clr r25 6254 2046 87FD sbrc r24,7 6255 2048 9095 com r25 6256 204a 97FF sbrs r25,7 6257 204c 03C0 rjmp .L413 6258 204e 9095 com r25 6259 2050 8195 neg r24 6260 2052 9F4F sbci r25,lo8(-1) 6261 .L413: 6262 2054 0297 sbiw r24,2 6263 2056 0CF0 brlt .+2 6264 2058 B2CF rjmp .L416 2436:srx1.c **** } 2437:srx1.c **** } 2438:srx1.c **** 2439:srx1.c **** // Stop, we're here! 2440:srx1.c **** ArbitrateCmd.info.output_l = POWER(0); // Stop motion 6266 .LM1071: 6267 205a 1092 0000 sts (ArbitrateCmd.16+8)+1,__zero_reg__ 6268 205e 1092 0000 sts ArbitrateCmd.16+8,__zero_reg__ 2441:srx1.c **** ArbitrateCmd.info.output_r = POWER(0); 6270 .LM1072: 6271 2062 1092 0000 sts (ArbitrateCmd.16+10)+1,__zero_reg__ 6272 2066 1092 0000 sts ArbitrateCmd.16+10,__zero_reg__ 2442:srx1.c **** //ArbitrateCmd.info.active = TRUE; 2443:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 6274 .LM1073: 6275 206a 60E0 ldi r22,lo8(ArbitrateCmd.16) 6276 206c 70E0 ldi r23,hi8(ArbitrateCmd.16) 6277 206e 80E0 ldi r24,lo8(ArbitrateQueue) 6278 2070 90E0 ldi r25,hi8(ArbitrateQueue) 6279 2072 0E94 0000 call AvrXSendMessage 2444:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 6281 .LM1074: 6282 2076 80E0 ldi r24,lo8(ArbitrateCmd.16) 6283 2078 90E0 ldi r25,hi8(ArbitrateCmd.16) 6284 207a 0E94 0000 call AvrXWaitMessageAck 2445:srx1.c **** 2446:srx1.c **** // Blow the darn thing out! 2447:srx1.c **** bit_set(FAN_CTL); 6286 .LM1075: 6287 207e 1A9A sbi 35-0x20,2 2448:srx1.c **** 2449:srx1.c **** // Blow for a fixed minimum time. (just in case fire is weakened by fan and SCM can't see it) 2450:srx1.c **** AvrXDelay(&extinguish_timer, FAN_DELAY); 6289 .LM1076: 6290 2080 60EC ldi r22,lo8(24000) 6291 2082 7DE5 ldi r23,hi8(24000) 6292 2084 80E0 ldi r24,lo8(extinguish_timer) 6293 2086 90E0 ldi r25,hi8(extinguish_timer) 6294 2088 0E94 0000 call AvrXDelay 2451:srx1.c **** 2452:srx1.c **** //while (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE) 2453:srx1.c **** // AvrXDelay(&extinguish_timer, MILLISECOND(50)); 2454:srx1.c **** 2455:srx1.c **** // Enough! 2456:srx1.c **** bit_clear(FAN_CTL); 6296 .LM1077: 6297 208c 1A98 cbi 35-0x20,2 2457:srx1.c **** 2458:srx1.c **** // Wait a while and then double-check if candle is out. 2459:srx1.c **** AvrXDelay(&extinguish_timer, SECOND(1)); 6299 .LM1078: 6300 208e 60E7 ldi r22,lo8(6000) 6301 2090 77E1 ldi r23,hi8(6000) 6302 2092 80E0 ldi r24,lo8(extinguish_timer) 6303 2094 90E0 ldi r25,hi8(extinguish_timer) 6304 2096 0E94 0000 call AvrXDelay 2460:srx1.c **** 2461:srx1.c **** count = EXTINGUISH_HUNT_ANGLE; 6306 .LM1079: 6307 209a C7E0 ldi r28,lo8(7) 2462:srx1.c **** 2463:srx1.c **** // Try stuff until the darn thing is out! 2464:srx1.c **** while (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE) 6309 .LM1080: 6310 209c 50C0 rjmp .L417 6311 .L422: 2465:srx1.c **** { 2466:srx1.c **** // If not extinguished, turn left and try again. 2467:srx1.c **** if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE) 6313 .LM1081: 6314 209e 62E0 ldi r22,lo8(2) 6315 20a0 80EB ldi r24,lo8(-80) 6316 20a2 0E94 0000 call i2c_readbyte 6317 20a6 8130 cpi r24,lo8(1) 6318 20a8 F9F4 brne .L420 2468:srx1.c **** { 2469:srx1.c **** RotateCmd.type = RELATIVE; 6320 .LM1082: 6321 20aa 1092 0000 sts RotateCmd.17+6,__zero_reg__ 2470:srx1.c **** RotateCmd.scan = FALSE; 6323 .LM1083: 6324 20ae 1092 0000 sts RotateCmd.17+7,__zero_reg__ 2471:srx1.c **** RotateCmd.angle = -count; 6326 .LM1084: 6327 20b2 8C2F mov r24,r28 6328 20b4 9927 clr r25 6329 20b6 9095 com r25 6330 20b8 8195 neg r24 6331 20ba 9F4F sbci r25,lo8(-1) 6332 20bc 9093 0000 sts (RotateCmd.17+4)+1,r25 6333 20c0 8093 0000 sts RotateCmd.17+4,r24 2472:srx1.c **** AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); 6335 .LM1085: 6336 20c4 60E0 ldi r22,lo8(RotateCmd.17) 6337 20c6 70E0 ldi r23,hi8(RotateCmd.17) 6338 20c8 80E0 ldi r24,lo8(RotateQueue) 6339 20ca 90E0 ldi r25,hi8(RotateQueue) 6340 20cc 0E94 0000 call AvrXSendMessage 2473:srx1.c **** AvrXWaitMessageAck(&RotateCmd.mcb); 6342 .LM1086: 6343 20d0 80E0 ldi r24,lo8(RotateCmd.17) 6344 20d2 90E0 ldi r25,hi8(RotateCmd.17) 6345 20d4 0E94 0000 call AvrXWaitMessageAck 2474:srx1.c **** 2475:srx1.c **** // Blow the darn thing out! 2476:srx1.c **** bit_set(FAN_CTL); 6347 .LM1087: 6348 20d8 1A9A sbi 35-0x20,2 2477:srx1.c **** 2478:srx1.c **** // Blow for a fixed minimum time. (just in case fire is weakened by fan and SCM can't see it 2479:srx1.c **** AvrXDelay(&extinguish_timer, FAN_DELAY); 6350 .LM1088: 6351 20da 60EC ldi r22,lo8(24000) 6352 20dc 7DE5 ldi r23,hi8(24000) 6353 20de 80E0 ldi r24,lo8(extinguish_timer) 6354 20e0 90E0 ldi r25,hi8(extinguish_timer) 6355 20e2 0E94 0000 call AvrXDelay 2480:srx1.c **** 2481:srx1.c **** // Enough! 2482:srx1.c **** bit_clear(FAN_CTL); 6357 .LM1089: 6358 20e6 1A98 cbi 35-0x20,2 6359 .L420: 2483:srx1.c **** } 2484:srx1.c **** 2485:srx1.c **** // Wait a while and then double-check if candle is out. 2486:srx1.c **** AvrXDelay(&extinguish_timer, SECOND(1)); 6361 .LM1090: 6362 20e8 60E7 ldi r22,lo8(6000) 6363 20ea 77E1 ldi r23,hi8(6000) 6364 20ec 80E0 ldi r24,lo8(extinguish_timer) 6365 20ee 90E0 ldi r25,hi8(extinguish_timer) 6366 20f0 0E94 0000 call AvrXDelay 2487:srx1.c **** 2488:srx1.c **** // Darn! still not out yet, try the right. 2489:srx1.c **** if (i2c_readbyte(SCM_I2C_ADDR, SCM_STATUS) == TRUE) 6368 .LM1091: 6369 20f4 62E0 ldi r22,lo8(2) 6370 20f6 80EB ldi r24,lo8(-80) 6371 20f8 0E94 0000 call i2c_readbyte 6372 20fc 8130 cpi r24,lo8(1) 6373 20fe F1F4 brne .L421 2490:srx1.c **** { 2491:srx1.c **** RotateCmd.type = RELATIVE; 6375 .LM1092: 6376 2100 1092 0000 sts RotateCmd.17+6,__zero_reg__ 2492:srx1.c **** RotateCmd.scan = FALSE; 6378 .LM1093: 6379 2104 1092 0000 sts RotateCmd.17+7,__zero_reg__ 2493:srx1.c **** RotateCmd.angle = count*2; 6381 .LM1094: 6382 2108 8C2F mov r24,r28 6383 210a 9927 clr r25 6384 210c 880F add r24,r24 6385 210e 991F adc r25,r25 6386 2110 9093 0000 sts (RotateCmd.17+4)+1,r25 6387 2114 8093 0000 sts RotateCmd.17+4,r24 2494:srx1.c **** AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); 6389 .LM1095: 6390 2118 60E0 ldi r22,lo8(RotateCmd.17) 6391 211a 70E0 ldi r23,hi8(RotateCmd.17) 6392 211c 80E0 ldi r24,lo8(RotateQueue) 6393 211e 90E0 ldi r25,hi8(RotateQueue) 6394 2120 0E94 0000 call AvrXSendMessage 2495:srx1.c **** AvrXWaitMessageAck(&RotateCmd.mcb); 6396 .LM1096: 6397 2124 80E0 ldi r24,lo8(RotateCmd.17) 6398 2126 90E0 ldi r25,hi8(RotateCmd.17) 6399 2128 0E94 0000 call AvrXWaitMessageAck 2496:srx1.c **** 2497:srx1.c **** // Blow the darn thing out! 2498:srx1.c **** bit_set(FAN_CTL); 6401 .LM1097: 6402 212c 1A9A sbi 35-0x20,2 2499:srx1.c **** 2500:srx1.c **** // Blow for a fixed minimum time. (just in case fire is weakened by fan and SCM can't see it 2501:srx1.c **** AvrXDelay(&extinguish_timer, FAN_DELAY); 6404 .LM1098: 6405 212e 60EC ldi r22,lo8(24000) 6406 2130 7DE5 ldi r23,hi8(24000) 6407 2132 80E0 ldi r24,lo8(extinguish_timer) 6408 2134 90E0 ldi r25,hi8(extinguish_timer) 6409 2136 0E94 0000 call AvrXDelay 2502:srx1.c **** 2503:srx1.c **** // Enough! 2504:srx1.c **** bit_clear(FAN_CTL); 6411 .LM1099: 6412 213a 1A98 cbi 35-0x20,2 6413 .L421: 2505:srx1.c **** } 2506:srx1.c **** 2507:srx1.c **** // Increase the angle 2508:srx1.c **** count += 1; 6415 .LM1100: 6416 213c CF5F subi r28,lo8(-(1)) 6417 .L417: 6418 213e 62E0 ldi r22,lo8(2) 6419 2140 80EB ldi r24,lo8(-80) 6420 2142 0E94 0000 call i2c_readbyte 6421 2146 8130 cpi r24,lo8(1) 6422 2148 09F4 brne .+2 6423 214a A9CF rjmp .L422 2509:srx1.c **** } 2510:srx1.c **** 2511:srx1.c **** // I guess we're done, report back. 2512:srx1.c **** ((ExtinguishMessage*)pExtinguishCmd)->extinguished = TRUE; 6425 .LM1101: 6426 214c 81E0 ldi r24,lo8(1) 6427 214e F701 movw r30,r14 6428 2150 8483 std Z+4,r24 2513:srx1.c **** 2514:srx1.c **** // We're done here. release. 2515:srx1.c **** ArbitrateCmd.info.output_l = POWER(0); // Stop motion 6430 .LM1102: 6431 2152 1092 0000 sts (ArbitrateCmd.16+8)+1,__zero_reg__ 6432 2156 1092 0000 sts ArbitrateCmd.16+8,__zero_reg__ 2516:srx1.c **** ArbitrateCmd.info.output_r = POWER(0); 6434 .LM1103: 6435 215a 1092 0000 sts (ArbitrateCmd.16+10)+1,__zero_reg__ 6436 215e 1092 0000 sts ArbitrateCmd.16+10,__zero_reg__ 2517:srx1.c **** ArbitrateCmd.info.active = FALSE; 6438 .LM1104: 6439 2162 1092 0000 sts ArbitrateCmd.16+6,__zero_reg__ 2518:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 6441 .LM1105: 6442 2166 60E0 ldi r22,lo8(ArbitrateCmd.16) 6443 2168 70E0 ldi r23,hi8(ArbitrateCmd.16) 6444 216a 80E0 ldi r24,lo8(ArbitrateQueue) 6445 216c 90E0 ldi r25,hi8(ArbitrateQueue) 6446 216e 0E94 0000 call AvrXSendMessage 2519:srx1.c **** AvrXWaitMessageAck(&ArbitrateCmd.mcb); 6448 .LM1106: 6449 2172 80E0 ldi r24,lo8(ArbitrateCmd.16) 6450 2174 90E0 ldi r25,hi8(ArbitrateCmd.16) 6451 2176 0E94 0000 call AvrXWaitMessageAck 2520:srx1.c **** 2521:srx1.c **** // report back! 2522:srx1.c **** AvrXAckMessage(pExtinguishCmd); 6453 .LM1107: 6454 217a C701 movw r24,r14 6455 217c 0E94 0000 call AvrXAckMessage 6456 2180 63CE rjmp .L423 2523:srx1.c **** } 2524:srx1.c **** } 6458 .LM1108: 6459 .LBE10: 6460 /* epilogue: frame size=0 */ 6461 /* epilogue: naked */ 6462 /* epilogue end (size=0) */ 6463 /* function extinguish size 420 (420) */ 6475 .Lscope9: 6477 .lcomm ForwardCmd.18,8 6478 .lcomm RotateCmd.19,9 6479 .lcomm AlignCmd.20,6 6481 .global search 6483 search: 2525:srx1.c **** 2526:srx1.c **** /****************************************** 2527:srx1.c **** Task - search 2528:srx1.c **** ******************************************/ 2529:srx1.c **** NAKEDFUNC(search) 2530:srx1.c **** { 6485 .LM1109: 6486 /* prologue: frame size=0 */ 6487 /* prologue: naked */ 6488 /* prologue end (size=0) */ 6489 .L438: 2531:srx1.c **** MessageControlBlock *pSearchCmd; 2532:srx1.c **** //static ArbitrateMessage ArbitrateCmd; // static is required. 2533:srx1.c **** static ForwardMessage ForwardCmd; 2534:srx1.c **** static RotateMessage RotateCmd; 2535:srx1.c **** static AlignMessage AlignCmd; 2536:srx1.c **** 2537:srx1.c **** // Reduce code by initializing it once only. 2538:srx1.c **** //ArbitrateCmd.id=SEARCH; 2539:srx1.c **** //ArbitrateCmd.info.type = ABSOLUTE; 2540:srx1.c **** //ArbitrateCmd.info.output_l = POWER(0); // No motion 2541:srx1.c **** //ArbitrateCmd.info.output_r = POWER(0); 2542:srx1.c **** 2543:srx1.c **** while (1) 2544:srx1.c **** { 2545:srx1.c **** pSearchCmd = AvrXWaitMessage(&SearchQueue); 6491 .LM1110: 6492 .LBB11: 6493 2182 80E0 ldi r24,lo8(SearchQueue) 6494 2184 90E0 ldi r25,hi8(SearchQueue) 6495 2186 0E94 0000 call AvrXWaitMessage 6496 218a EC01 movw r28,r24 2546:srx1.c **** 2547:srx1.c **** // Setup status 2548:srx1.c **** ((SearchMessage*)pSearchCmd)->status = FALSE; 6498 .LM1111: 6499 218c 1C82 std Y+4,__zero_reg__ 2549:srx1.c **** 2550:srx1.c **** // We should already be aligned with door frame so move forward a fixed amount 2551:srx1.c **** ForwardCmd.lenght = SEARCH_ENTER_DISTANCE; // Move forward enough so the whole base is inside th 6501 .LM1112: 6502 218e 80E1 ldi r24,lo8(16) 6503 2190 90E0 ldi r25,hi8(16) 6504 2192 9093 0000 sts (ForwardCmd.18+4)+1,r25 6505 2196 8093 0000 sts ForwardCmd.18+4,r24 2552:srx1.c **** ForwardCmd.ignore_door = TRUE; 6507 .LM1113: 6508 219a 11E0 ldi r17,lo8(1) 6509 219c 1093 0000 sts ForwardCmd.18+6,r17 2553:srx1.c **** AvrXSendMessage(&ForwardQueue, &ForwardCmd.mcb); 6511 .LM1114: 6512 21a0 60E0 ldi r22,lo8(ForwardCmd.18) 6513 21a2 70E0 ldi r23,hi8(ForwardCmd.18) 6514 21a4 80E0 ldi r24,lo8(ForwardQueue) 6515 21a6 90E0 ldi r25,hi8(ForwardQueue) 6516 21a8 0E94 0000 call AvrXSendMessage 2554:srx1.c **** AvrXWaitMessageAck(&ForwardCmd.mcb); 6518 .LM1115: 6519 21ac 80E0 ldi r24,lo8(ForwardCmd.18) 6520 21ae 90E0 ldi r25,hi8(ForwardCmd.18) 6521 21b0 0E94 0000 call AvrXWaitMessageAck 2555:srx1.c **** 2556:srx1.c **** // Align with adjacent wall to ensure we do an exact 180 degrees scan. 2557:srx1.c **** if (((SearchMessage*)pSearchCmd)->side == RIGHT) 6523 .LM1116: 6524 21b4 8D81 ldd r24,Y+5 6525 21b6 8117 cp r24,r17 6526 21b8 19F4 brne .L433 2558:srx1.c **** AlignCmd.type = ALIGN_LEFT; 6528 .LM1117: 6529 21ba 1092 0000 sts AlignCmd.20+4,__zero_reg__ 6530 21be 02C0 rjmp .L434 6531 .L433: 2559:srx1.c **** else 2560:srx1.c **** AlignCmd.type = ALIGN_RIGHT; 6533 .LM1118: 6534 21c0 1093 0000 sts AlignCmd.20+4,r17 6535 .L434: 2561:srx1.c **** 2562:srx1.c **** AvrXSendMessage(&AlignQueue, &AlignCmd.mcb); 6537 .LM1119: 6538 21c4 60E0 ldi r22,lo8(AlignCmd.20) 6539 21c6 70E0 ldi r23,hi8(AlignCmd.20) 6540 21c8 80E0 ldi r24,lo8(AlignQueue) 6541 21ca 90E0 ldi r25,hi8(AlignQueue) 6542 21cc 0E94 0000 call AvrXSendMessage 2563:srx1.c **** AvrXWaitMessageAck(&AlignCmd.mcb); 6544 .LM1120: 6545 21d0 80E0 ldi r24,lo8(AlignCmd.20) 6546 21d2 90E0 ldi r25,hi8(AlignCmd.20) 6547 21d4 0E94 0000 call AvrXWaitMessageAck 2564:srx1.c **** 2565:srx1.c **** // Reset Odometry variables 2566:srx1.c **** ResetOdometry(); 6549 .LM1121: 6550 21d8 0E94 0000 call ResetOdometry 2567:srx1.c **** 2568:srx1.c **** // Scan room and signal PLANNER if candle was found. 2569:srx1.c **** RotateCmd.type = RELATIVE; 6552 .LM1122: 6553 21dc 1092 0000 sts RotateCmd.19+6,__zero_reg__ 2570:srx1.c **** RotateCmd.angle = (((SearchMessage*)pSearchCmd)->side == RIGHT) ? +180 : -180; // which way? 6555 .LM1123: 6556 21e0 8D81 ldd r24,Y+5 6557 21e2 8130 cpi r24,lo8(1) 6558 21e4 19F4 brne .L435 6559 21e6 84EB ldi r24,lo8(180) 6560 21e8 90E0 ldi r25,hi8(180) 6561 21ea 02C0 rjmp .L436 6562 .L435: 6563 21ec 8CE4 ldi r24,lo8(-180) 6564 21ee 9FEF ldi r25,hi8(-180) 6565 .L436: 6566 21f0 9093 0000 sts (RotateCmd.19+4)+1,r25 6567 21f4 8093 0000 sts RotateCmd.19+4,r24 2571:srx1.c **** RotateCmd.scan = TRUE; 6569 .LM1124: 6570 21f8 81E0 ldi r24,lo8(1) 6571 21fa 8093 0000 sts RotateCmd.19+7,r24 2572:srx1.c **** 2573:srx1.c **** AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); 6573 .LM1125: 6574 21fe 60E0 ldi r22,lo8(RotateCmd.19) 6575 2200 70E0 ldi r23,hi8(RotateCmd.19) 6576 2202 80E0 ldi r24,lo8(RotateQueue) 6577 2204 90E0 ldi r25,hi8(RotateQueue) 6578 2206 0E94 0000 call AvrXSendMessage 2574:srx1.c **** AvrXWaitMessageAck(&RotateCmd.mcb); 6580 .LM1126: 6581 220a 80E0 ldi r24,lo8(RotateCmd.19) 6582 220c 90E0 ldi r25,hi8(RotateCmd.19) 6583 220e 0E94 0000 call AvrXWaitMessageAck 2575:srx1.c **** 2576:srx1.c **** // Was candle found? 2577:srx1.c **** if (RotateCmd.status == TRUE) 6585 .LM1127: 6586 2212 1091 0000 lds r17,RotateCmd.19+8 6587 2216 1130 cpi r17,lo8(1) 6588 2218 21F4 brne .L437 2578:srx1.c **** { 2579:srx1.c **** PutChar('!'); 6590 .LM1128: 6591 221a 81E2 ldi r24,lo8(33) 6592 221c 0E94 0000 call PutChar 2580:srx1.c **** ((SearchMessage*)pSearchCmd)->status = TRUE; 6594 .LM1129: 6595 2220 1C83 std Y+4,r17 6596 .L437: 2581:srx1.c **** } 2582:srx1.c **** 2583:srx1.c **** // We're done here. Report back to PLANNER. 2584:srx1.c **** //ArbitrateCmd.info.active = FALSE; 2585:srx1.c **** //AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 2586:srx1.c **** //AvrXWaitMessageAck(&ArbitrateCmd.mcb); 2587:srx1.c **** 2588:srx1.c **** // report back! 2589:srx1.c **** AvrXAckMessage(pSearchCmd); 6598 .LM1130: 6599 2222 CE01 movw r24,r28 6600 2224 0E94 0000 call AvrXAckMessage 6601 2228 ACCF rjmp .L438 2590:srx1.c **** } 2591:srx1.c **** } 6603 .LM1131: 6604 .LBE11: 6605 /* epilogue: frame size=0 */ 6606 /* epilogue: naked */ 6607 /* epilogue end (size=0) */ 6608 /* function search size 84 (84) */ 6616 .Lscope10: 6618 .lcomm pArbitrateCmd.21,2 6619 .lcomm ArbitrateCmd.22,14 6621 .global arbitrate 6623 arbitrate: 2592:srx1.c **** 2593:srx1.c **** /****************************************** 2594:srx1.c **** Task - arbitrate 2595:srx1.c **** ******************************************/ 2596:srx1.c **** NAKEDFUNC(arbitrate) 2597:srx1.c **** { 6625 .LM1132: 6626 /* prologue: frame size=0 */ 6627 /* prologue: naked */ 6628 /* prologue end (size=0) */ 2598:srx1.c **** static MessageControlBlock *pArbitrateCmd; // static is required. 2599:srx1.c **** static ArbitrateMessage ArbitrateCmd; // For dummy message to ourself. 2600:srx1.c **** 2601:srx1.c **** unsigned char id; 2602:srx1.c **** unsigned char priority; 2603:srx1.c **** unsigned char exit; 2604:srx1.c **** 2605:srx1.c **** 2606:srx1.c **** //#define ARBITRATE_DEBUG 1 2607:srx1.c **** //#define ARBITRATE_DEBUG2 1 2608:srx1.c **** 2609:srx1.c **** // Send ourselves a dummy message to have the arbitator kick start arbitrator. 2610:srx1.c **** ArbitrateCmd.id = ROAM; 6630 .LM1133: 6631 .LBB12: 6632 222a 86E0 ldi r24,lo8(6) 6633 222c 8093 0000 sts ArbitrateCmd.22+4,r24 2611:srx1.c **** ArbitrateCmd.info.type = ABSOLUTE; 6635 .LM1134: 6636 2230 81E0 ldi r24,lo8(1) 6637 2232 8093 0000 sts ArbitrateCmd.22+13,r24 2612:srx1.c **** ArbitrateCmd.info.active = FALSE; 6639 .LM1135: 6640 2236 1092 0000 sts ArbitrateCmd.22+6,__zero_reg__ 2613:srx1.c **** ArbitrateCmd.info.enable = FALSE; 6642 .LM1136: 6643 223a 1092 0000 sts ArbitrateCmd.22+5,__zero_reg__ 2614:srx1.c **** ArbitrateCmd.info.output_l = POWER(56); 6645 .LM1137: 6646 223e 86E1 ldi r24,lo8(22) 6647 2240 90E0 ldi r25,hi8(22) 6648 2242 9093 0000 sts (ArbitrateCmd.22+8)+1,r25 6649 2246 8093 0000 sts ArbitrateCmd.22+8,r24 2615:srx1.c **** ArbitrateCmd.info.output_r = POWER(56); 6651 .LM1138: 6652 224a 9093 0000 sts (ArbitrateCmd.22+10)+1,r25 6653 224e 8093 0000 sts ArbitrateCmd.22+10,r24 2616:srx1.c **** AvrXSendMessage(&ArbitrateQueue, &ArbitrateCmd.mcb); 6655 .LM1139: 6656 2252 60E0 ldi r22,lo8(ArbitrateCmd.22) 6657 2254 70E0 ldi r23,hi8(ArbitrateCmd.22) 6658 2256 80E0 ldi r24,lo8(ArbitrateQueue) 6659 2258 90E0 ldi r25,hi8(ArbitrateQueue) 6660 225a 0E94 0000 call AvrXSendMessage 6661 .L466: 2617:srx1.c **** 2618:srx1.c **** while (1) 2619:srx1.c **** { 2620:srx1.c **** // Wait for a behaviour to send us us an update. 2621:srx1.c **** pArbitrateCmd = AvrXWaitMessage(&ArbitrateQueue); 6663 .LM1140: 6664 225e 80E0 ldi r24,lo8(ArbitrateQueue) 6665 2260 90E0 ldi r25,hi8(ArbitrateQueue) 6666 2262 0E94 0000 call AvrXWaitMessage 6667 2266 9093 0000 sts (pArbitrateCmd.21)+1,r25 6668 226a 8093 0000 sts pArbitrateCmd.21,r24 2622:srx1.c **** 2623:srx1.c **** // Debug 2624:srx1.c **** if (arbitrate_debug) 6670 .LM1141: 6671 226e 8091 0000 lds r24,arbitrate_debug 6672 2272 8823 tst r24 6673 2274 69F0 breq .L443 2625:srx1.c **** { 2626:srx1.c **** PutChar('('); 6675 .LM1142: 6676 2276 88E2 ldi r24,lo8(40) 6677 2278 0E94 0000 call PutChar 2627:srx1.c **** PutDecByte(((ArbitrateMessage*)pArbitrateCmd)->id); 6679 .LM1143: 6680 227c E091 0000 lds r30,pArbitrateCmd.21 6681 2280 F091 0000 lds r31,(pArbitrateCmd.21)+1 6682 2284 8481 ldd r24,Z+4 6683 2286 0E94 0000 call PutDecByte 2628:srx1.c **** PutChar('>'); 6685 .LM1144: 6686 228a 8EE3 ldi r24,lo8(62) 6687 228c 0E94 0000 call PutChar 6688 .L443: 2629:srx1.c **** } 2630:srx1.c **** 2631:srx1.c **** // Check for invalid behaviour id 2632:srx1.c **** if (((ArbitrateMessage*)pArbitrateCmd)->id > BEHAVIOUR_COUNT) 6690 .LM1145: 6691 2290 A091 0000 lds r26,pArbitrateCmd.21 6692 2294 B091 0000 lds r27,(pArbitrateCmd.21)+1 6693 2298 1496 adiw r26,4 6694 229a 8C91 ld r24,X 6695 229c 1497 sbiw r26,4 6696 229e 8B30 cpi r24,lo8(11) 6697 22a0 20F0 brlo .L444 2633:srx1.c **** { 2634:srx1.c **** AvrXAckMessage(pArbitrateCmd); 6699 .LM1146: 6700 22a2 CD01 movw r24,r26 6701 22a4 0E94 0000 call AvrXAckMessage 2635:srx1.c **** continue; 6703 .LM1147: 6704 22a8 DACF rjmp .L466 6705 .L444: 2636:srx1.c **** } 2637:srx1.c **** 2638:srx1.c **** // Update private table 2639:srx1.c **** // (NB: the 'enable' parameter is not updated as only the EXEC and PLANNER should be allowed to c 2640:srx1.c **** BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].active = ((ArbitrateMessage*)pArbitrateCmd 6707 .LM1148: 6708 22aa 9927 clr r25 6709 22ac FC01 movw r30,r24 6710 22ae 53E0 ldi r21,3 6711 22b0 EE0F 1: lsl r30 6712 22b2 FF1F rol r31 6713 22b4 5A95 dec r21 6714 22b6 E1F7 brne 1b 6715 22b8 E80F add r30,r24 6716 22ba F91F adc r31,r25 6717 22bc E050 subi r30,lo8(-(BehaviourTable)) 6718 22be F040 sbci r31,hi8(-(BehaviourTable)) 6719 22c0 1696 adiw r26,6 6720 22c2 8C91 ld r24,X 6721 22c4 1697 sbiw r26,6 6722 22c6 8183 std Z+1,r24 2641:srx1.c **** BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].output_l = ((ArbitrateMessage*)pArbitrateC 6724 .LM1149: 6725 22c8 1496 adiw r26,4 6726 22ca 8C91 ld r24,X 6727 22cc 1497 sbiw r26,4 6728 22ce 9927 clr r25 6729 22d0 FC01 movw r30,r24 6730 22d2 43E0 ldi r20,3 6731 22d4 EE0F 1: lsl r30 6732 22d6 FF1F rol r31 6733 22d8 4A95 dec r20 6734 22da E1F7 brne 1b 6735 22dc E80F add r30,r24 6736 22de F91F adc r31,r25 6737 22e0 E050 subi r30,lo8(-(BehaviourTable)) 6738 22e2 F040 sbci r31,hi8(-(BehaviourTable)) 6739 22e4 1896 adiw r26,8 6740 22e6 8D91 ld r24,X+ 6741 22e8 9C91 ld r25,X 6742 22ea 1997 sbiw r26,8+1 6743 22ec 8383 std Z+3,r24 6744 22ee 9483 std Z+4,r25 2642:srx1.c **** BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].output_r = ((ArbitrateMessage*)pArbitrateC 6746 .LM1150: 6747 22f0 1496 adiw r26,4 6748 22f2 8C91 ld r24,X 6749 22f4 1497 sbiw r26,4 6750 22f6 9927 clr r25 6751 22f8 FC01 movw r30,r24 6752 22fa 33E0 ldi r19,3 6753 22fc EE0F 1: lsl r30 6754 22fe FF1F rol r31 6755 2300 3A95 dec r19 6756 2302 E1F7 brne 1b 6757 2304 E80F add r30,r24 6758 2306 F91F adc r31,r25 6759 2308 E050 subi r30,lo8(-(BehaviourTable)) 6760 230a F040 sbci r31,hi8(-(BehaviourTable)) 6761 230c 1A96 adiw r26,10 6762 230e 8D91 ld r24,X+ 6763 2310 9C91 ld r25,X 6764 2312 1B97 sbiw r26,10+1 6765 2314 8583 std Z+5,r24 6766 2316 9683 std Z+6,r25 2643:srx1.c **** BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].type = ((ArbitrateMessage*)pArbitrateCmd)- 6768 .LM1151: 6769 2318 1496 adiw r26,4 6770 231a 8C91 ld r24,X 6771 231c 1497 sbiw r26,4 6772 231e 9927 clr r25 6773 2320 FC01 movw r30,r24 6774 2322 23E0 ldi r18,3 6775 2324 EE0F 1: lsl r30 6776 2326 FF1F rol r31 6777 2328 2A95 dec r18 6778 232a E1F7 brne 1b 6779 232c E80F add r30,r24 6780 232e F91F adc r31,r25 6781 2330 E050 subi r30,lo8(-(BehaviourTable)) 6782 2332 F040 sbci r31,hi8(-(BehaviourTable)) 6783 2334 1D96 adiw r26,13 6784 2336 8C91 ld r24,X 6785 2338 1D97 sbiw r26,13 6786 233a 8087 std Z+8,r24 2644:srx1.c **** BehaviourTable[((ArbitrateMessage*)pArbitrateCmd)->id].precision = ((ArbitrateMessage*)pArbitrate 6788 .LM1152: 6789 233c 1496 adiw r26,4 6790 233e 8C91 ld r24,X 6791 2340 1497 sbiw r26,4 6792 2342 9927 clr r25 6793 2344 FC01 movw r30,r24 6794 2346 03E0 ldi r16,3 6795 2348 EE0F 1: lsl r30 6796 234a FF1F rol r31 6797 234c 0A95 dec r16 6798 234e E1F7 brne 1b 6799 2350 E80F add r30,r24 6800 2352 F91F adc r31,r25 6801 2354 E050 subi r30,lo8(-(BehaviourTable)) 6802 2356 F040 sbci r31,hi8(-(BehaviourTable)) 6803 2358 1C96 adiw r26,12 6804 235a 8C91 ld r24,X 6805 235c 1C97 sbiw r26,12 6806 235e 8783 std Z+7,r24 2645:srx1.c **** 2646:srx1.c **** // We have all the information we needed; acknowledge message. 2647:srx1.c **** AvrXAckMessage(pArbitrateCmd); 6808 .LM1153: 6809 2360 CD01 movw r24,r26 6810 2362 0E94 0000 call AvrXAckMessage 2648:srx1.c **** 2649:srx1.c **** exit = FALSE; 6812 .LM1154: 6813 2366 20E0 ldi r18,lo8(0) 2650:srx1.c **** 2651:srx1.c **** for (priority = 0; priority < BEHAVIOUR_PRIORITY_MAX && exit == FALSE ; priority++) 6815 .LM1155: 6816 2368 022F mov r16,r18 6817 .L458: 2652:srx1.c **** { 2653:srx1.c **** for (id = 0; id < BEHAVIOUR_COUNT && exit == FALSE ; id++) 6819 .LM1156: 6820 236a C0E0 ldi r28,lo8(0) 6821 236c 2223 tst r18 6822 236e 71F5 brne .L451 6823 .L457: 2654:srx1.c **** { 2655:srx1.c **** if (BehaviourTable[id].enable == TRUE && BehaviourTable[id].priority == priority && BehaviourTa 6825 .LM1157: 6826 2370 8C2F mov r24,r28 6827 2372 9927 clr r25 6828 2374 FC01 movw r30,r24 6829 2376 13E0 ldi r17,3 6830 2378 EE0F 1: lsl r30 6831 237a FF1F rol r31 6832 237c 1A95 dec r17 6833 237e E1F7 brne 1b 6834 2380 E80F add r30,r24 6835 2382 F91F adc r31,r25 6836 2384 E050 subi r30,lo8(-(BehaviourTable)) 6837 2386 F040 sbci r31,hi8(-(BehaviourTable)) 6838 2388 8081 ld r24,Z 6839 238a 8130 cpi r24,lo8(1) 6840 238c D1F4 brne .L452 6841 238e 8281 ldd r24,Z+2 6842 2390 8017 cp r24,r16 6843 2392 B9F4 brne .L452 6844 2394 1181 ldd r17,Z+1 6845 2396 1130 cpi r17,lo8(1) 6846 2398 A1F4 brne .L452 2656:srx1.c **** { 2657:srx1.c **** motors(&BehaviourTable[id]); // Pass command to PID controller. 6848 .LM1158: 6849 239a 89E0 ldi r24,lo8(9) 6850 239c C89F mul r28,r24 6851 239e C001 movw r24,r0 6852 23a0 1124 clr r1 6853 23a2 8050 subi r24,lo8(-(BehaviourTable)) 6854 23a4 9040 sbci r25,hi8(-(BehaviourTable)) 6855 23a6 0E94 0000 call motors 2658:srx1.c **** 2659:srx1.c **** // debug 2660:srx1.c **** if (arbitrate_debug) 6857 .LM1159: 6858 23aa 8091 0000 lds r24,arbitrate_debug 6859 23ae 8823 tst r24 6860 23b0 39F0 breq .L456 2661:srx1.c **** { 2662:srx1.c **** PutChar(id+'0'); 6862 .LM1160: 6863 23b2 8C2F mov r24,r28 6864 23b4 805D subi r24,lo8(-(48)) 6865 23b6 0E94 0000 call PutChar 2663:srx1.c **** PutChar(')'); 6867 .LM1161: 6868 23ba 89E2 ldi r24,lo8(41) 6869 23bc 0E94 0000 call PutChar 6870 .L456: 2664:srx1.c **** } 2665:srx1.c **** 2666:srx1.c **** #ifdef ARBITRATE_DEBUG2 2667:srx1.c **** PutChar('['); 2668:srx1.c **** PutDecSignedByte(BehaviourTable[id].output_l); 2669:srx1.c **** PutChar(','); 2670:srx1.c **** PutDecSignedByte(BehaviourTable[id].output_r); 2671:srx1.c **** PutChar(']'); 2672:srx1.c **** #endif 2673:srx1.c **** exit = TRUE; // can't I use a simple break command? 6872 .LM1162: 6873 23c0 212F mov r18,r17 6875 .LM1163: 6876 .L452: 6877 23c2 CF5F subi r28,lo8(-(1)) 6878 23c4 CA30 cpi r28,lo8(10) 6879 23c6 10F4 brsh .L451 6880 23c8 2223 tst r18 6881 23ca 91F2 breq .L457 6882 .L451: 6884 .LM1164: 6885 23cc 0F5F subi r16,lo8(-(1)) 6886 23ce 0930 cpi r16,lo8(9) 6887 23d0 10F4 brsh .L446 6888 23d2 2223 tst r18 6889 23d4 51F2 breq .L458 6890 .L446: 2674:srx1.c **** 2675:srx1.c **** } 2676:srx1.c **** } 2677:srx1.c **** } 2678:srx1.c **** 2679:srx1.c **** // if nobody was found to be active make sure we default to STOP! 2680:srx1.c **** if (exit == FALSE) 6892 .LM1165: 6893 23d6 2223 tst r18 6894 23d8 79F4 brne .L459 2681:srx1.c **** { 2682:srx1.c **** // Debug 2683:srx1.c **** if (arbitrate_debug) 6896 .LM1166: 6897 23da 8091 0000 lds r24,arbitrate_debug 6898 23de 8823 tst r24 6899 23e0 19F0 breq .L460 2684:srx1.c **** PutChar('-'); 6901 .LM1167: 6902 23e2 8DE2 ldi r24,lo8(45) 6903 23e4 0E94 0000 call PutChar 6904 .L460: 2685:srx1.c **** 2686:srx1.c **** Left.VelocitySetpoint = 0; 6906 .LM1168: 6907 23e8 1092 0000 sts (Left+14)+1,__zero_reg__ 6908 23ec 1092 0000 sts Left+14,__zero_reg__ 2687:srx1.c **** Right.VelocitySetpoint = 0; 6910 .LM1169: 6911 23f0 1092 0000 sts (Right+14)+1,__zero_reg__ 6912 23f4 1092 0000 sts Right+14,__zero_reg__ 6913 .L459: 2688:srx1.c **** } 2689:srx1.c **** 2690:srx1.c **** // Show user we are still alive and well! 2691:srx1.c **** bit_toggle(HEARTBEAT_LED); 6915 .LM1170: 6916 23f8 C79B sbis 56-0x20,7 6917 23fa 02C0 rjmp .L461 6918 23fc C798 cbi 56-0x20,7 6919 23fe 2FCF rjmp .L466 6920 .L461: 6921 2400 C79A sbi 56-0x20,7 6922 2402 2DCF rjmp .L466 2692:srx1.c **** } 2693:srx1.c **** } 6924 .LM1171: 6925 .LBE12: 6926 /* epilogue: frame size=0 */ 6927 /* epilogue: naked */ 6928 /* epilogue end (size=0) */ 6929 /* function arbitrate size 237 (237) */ 6938 .Lscope11: 6941 .global InitBehaviourTable 6943 InitBehaviourTable: 2694:srx1.c **** 2695:srx1.c **** // Function Name: InitBehaviourTable() 2696:srx1.c **** // 2697:srx1.c **** // Description: Initialize Behaviour table to known values. 2698:srx1.c **** // 2699:srx1.c **** // 2700:srx1.c **** void InitBehaviourTable(void) 2701:srx1.c **** { 6945 .LM1172: 6946 /* prologue: frame size=0 */ 6947 /* prologue end (size=0) */ 2702:srx1.c **** unsigned char id; 2703:srx1.c **** 2704:srx1.c **** for (id = 0; id < BEHAVIOUR_COUNT ; id++) 6949 .LM1173: 6950 .LBB13: 6951 2404 90E0 ldi r25,lo8(0) 6952 2406 E0E0 ldi r30,lo8(BehaviourTable) 6953 2408 F0E0 ldi r31,hi8(BehaviourTable) 6954 .L472: 2705:srx1.c **** { 2706:srx1.c **** BehaviourTable[id].active = FALSE; 6956 .LM1174: 6957 240a 1182 std Z+1,__zero_reg__ 2707:srx1.c **** BehaviourTable[id].enable = FALSE; 6959 .LM1175: 6960 240c 1082 st Z,__zero_reg__ 2708:srx1.c **** BehaviourTable[id].output_l = 0; 6962 .LM1176: 6963 240e 1382 std Z+3,__zero_reg__ 6964 2410 1482 std Z+4,__zero_reg__ 2709:srx1.c **** BehaviourTable[id].output_r = 0; 6966 .LM1177: 6967 2412 1582 std Z+5,__zero_reg__ 6968 2414 1682 std Z+6,__zero_reg__ 2710:srx1.c **** BehaviourTable[id].type = ABSOLUTE; 6970 .LM1178: 6971 2416 81E0 ldi r24,lo8(1) 6972 2418 8087 std Z+8,r24 2711:srx1.c **** BehaviourTable[id].precision = FALSE; // default is OFF! 6974 .LM1179: 6975 241a 1782 std Z+7,__zero_reg__ 6977 .LM1180: 6978 241c 980F add r25,r24 6979 241e 3996 adiw r30,9 6980 2420 9A30 cpi r25,lo8(10) 6981 2422 98F3 brlo .L472 2712:srx1.c **** } 2713:srx1.c **** 2714:srx1.c **** // Those Behaviours should always be enabled 2715:srx1.c **** BehaviourTable[EXEC].enable = TRUE; 6983 .LM1181: 6984 2424 8093 0000 sts BehaviourTable,r24 2716:srx1.c **** BehaviourTable[FOLLOW].enable = TRUE; 6986 .LM1182: 6987 2428 8093 0000 sts BehaviourTable+9,r24 2717:srx1.c **** BehaviourTable[GOAT].enable = TRUE; 6989 .LM1183: 6990 242c 8093 0000 sts BehaviourTable+36,r24 2718:srx1.c **** BehaviourTable[ROTATE].enable = TRUE; 6992 .LM1184: 6993 2430 8093 0000 sts BehaviourTable+18,r24 2719:srx1.c **** BehaviourTable[FORWARD].enable = TRUE; 6995 .LM1185: 6996 2434 8093 0000 sts BehaviourTable+27,r24 2720:srx1.c **** BehaviourTable[EXTINGUISH].enable = TRUE; 6998 .LM1186: 6999 2438 8093 0000 sts BehaviourTable+63,r24 2721:srx1.c **** BehaviourTable[SEARCH].enable = TRUE; 7001 .LM1187: 7002 243c 8093 0000 sts BehaviourTable+72,r24 2722:srx1.c **** BehaviourTable[ALIGN].enable = TRUE; 7004 .LM1188: 7005 2440 8093 0000 sts BehaviourTable+81,r24 2723:srx1.c **** BehaviourTable[AVOID].enable = TRUE; 7007 .LM1189: 7008 2444 8093 0000 sts BehaviourTable+45,r24 2724:srx1.c **** 2725:srx1.c **** // Initial behavioural priority (the lower numbers have higher priority) 2726:srx1.c **** BehaviourTable[EXEC].priority = 1; 7010 .LM1190: 7011 2448 8093 0000 sts BehaviourTable+2,r24 2727:srx1.c **** BehaviourTable[AVOID].priority = 2; 7013 .LM1191: 7014 244c 82E0 ldi r24,lo8(2) 7015 244e 8093 0000 sts BehaviourTable+47,r24 2728:srx1.c **** BehaviourTable[FOLLOW].priority = 3; 7017 .LM1192: 7018 2452 83E0 ldi r24,lo8(3) 7019 2454 8093 0000 sts BehaviourTable+11,r24 2729:srx1.c **** BehaviourTable[GOAT].priority = 6; 7021 .LM1193: 7022 2458 96E0 ldi r25,lo8(6) 7023 245a 9093 0000 sts BehaviourTable+38,r25 2730:srx1.c **** BehaviourTable[ROTATE].priority = 4; 7025 .LM1194: 7026 245e 84E0 ldi r24,lo8(4) 7027 2460 8093 0000 sts BehaviourTable+20,r24 2731:srx1.c **** BehaviourTable[FORWARD].priority = 4; 7029 .LM1195: 7030 2464 8093 0000 sts BehaviourTable+29,r24 2732:srx1.c **** BehaviourTable[ROAM].priority = 8; // the lowest priority 7032 .LM1196: 7033 2468 88E0 ldi r24,lo8(8) 7034 246a 8093 0000 sts BehaviourTable+56,r24 2733:srx1.c **** BehaviourTable[EXTINGUISH].priority = 6; 7036 .LM1197: 7037 246e 9093 0000 sts BehaviourTable+65,r25 2734:srx1.c **** BehaviourTable[SEARCH].priority = 6; 7039 .LM1198: 7040 2472 9093 0000 sts BehaviourTable+74,r25 2735:srx1.c **** BehaviourTable[ALIGN].priority = 6; 7042 .LM1199: 7043 2476 9093 0000 sts BehaviourTable+83,r25 2736:srx1.c **** } 7045 .LM1200: 7046 .LBE13: 7047 /* epilogue: frame size=0 */ 7048 247a 0895 ret 7049 /* epilogue end (size=1) */ 7050 /* function InitBehaviourTable size 60 (59) */ 7055 .Lscope12: 7058 .global main 7060 main: 2737:srx1.c **** 2738:srx1.c **** // Function Name: main() 2739:srx1.c **** // 2740:srx1.c **** // Description: Setup stuff on startup 2741:srx1.c **** // 2742:srx1.c **** // 2743:srx1.c **** int main(void) // Main runs under the AvrX Stack 2744:srx1.c **** { 7062 .LM1201: 7063 /* prologue: frame size=0 */ 7064 247c C0E0 ldi r28,lo8(__stack - 0) 7065 247e D0E0 ldi r29,hi8(__stack - 0) 7066 2480 DEBF out __SP_H__,r29 7067 2482 CDBF out __SP_L__,r28 7068 /* prologue end (size=4) */ 2745:srx1.c **** AvrXSetKernelStack(0); // Configure Kernel Stack 7070 .LM1202: 7071 2484 80E0 ldi r24,lo8(0) 7072 2486 90E0 ldi r25,hi8(0) 7073 2488 0E94 0000 call AvrXSetKernelStack 2746:srx1.c **** 2747:srx1.c **** MCUCR = MCUCR_INIT; // Enable "Sleep" instruction (is done with external ints. on old megas) 7075 .LM1203: 7076 248c 30E2 ldi r19,lo8(32) 7077 248e 35BF out 85-0x20,r19 2748:srx1.c **** 2749:srx1.c **** TCNT0 = TCNT0_INIT; // Initialize RTC 7079 .LM1204: 7080 2490 86EF ldi r24,lo8(-10) 7081 2492 82BF out 82-0x20,r24 2750:srx1.c **** TCCR0 = TCCR0_INIT; // Set up Timer0 for CLK/256 rate 7083 .LM1205: 7084 2494 96E0 ldi r25,lo8(6) 7085 2496 93BF out 83-0x20,r25 2751:srx1.c **** TIMSK = TIMSK_INIT; // Enable Timer0 overflow interrupt 7087 .LM1206: 7088 2498 21E0 ldi r18,lo8(1) 7089 249a 27BF out 87-0x20,r18 2752:srx1.c **** 2753:srx1.c **** // Configure Ports (see I/O assignment map in hardware.h) 2754:srx1.c **** DDRA = DDRA_INIT; 7091 .LM1207: 7092 249c 8FEF ldi r24,lo8(-1) 7093 249e 8ABB out 58-0x20,r24 2755:srx1.c **** PORTA = PORTA_INIT; 7095 .LM1208: 7096 24a0 1BBA out 59-0x20,__zero_reg__ 2756:srx1.c **** 2757:srx1.c **** DDRB = DDRB_INIT; 7098 .LM1209: 7099 24a2 87BB out 55-0x20,r24 2758:srx1.c **** PORTB = PORTB_INIT; 7101 .LM1210: 7102 24a4 18BA out 56-0x20,__zero_reg__ 2759:srx1.c **** 2760:srx1.c **** DDRC = DDRC_INIT; 7104 .LM1211: 7105 24a6 84BB out 52-0x20,r24 2761:srx1.c **** PORTC = PORTC_INIT; 7107 .LM1212: 7108 24a8 15BA out 53-0x20,__zero_reg__ 2762:srx1.c **** 2763:srx1.c **** DDRD = DDRD_INIT; 7110 .LM1213: 7111 24aa 8CE3 ldi r24,lo8(60) 7112 24ac 81BB out 49-0x20,r24 2764:srx1.c **** PORTD = PORTD_INIT; 7114 .LM1214: 7115 24ae 12BA out 50-0x20,__zero_reg__ 2765:srx1.c **** 2766:srx1.c **** DDRE = DDRE_INIT; 7117 .LM1215: 7118 24b0 92B9 out 34-0x20,r25 2767:srx1.c **** PORTE = PORTE_INIT; 7120 .LM1216: 7121 24b2 13B8 out 35-0x20,__zero_reg__ 2768:srx1.c **** 2769:srx1.c **** DDRF = DDRF_INIT; 7123 .LM1217: 7124 24b4 3093 6100 sts 97,r19 2770:srx1.c **** PORTF = PORTF_INIT; 7126 .LM1218: 7127 24b8 1092 6200 sts 98,__zero_reg__ 2771:srx1.c **** 2772:srx1.c **** // Setup PWM 2773:srx1.c **** TCCR1A = TCCR1A_INIT; // Setup 8-bit non-inverted PWM mode for Timer1 at ~30Khz. 7129 .LM1219: 7130 24bc 81EA ldi r24,lo8(-95) 7131 24be 8FBD out 79-0x20,r24 2774:srx1.c **** TCCR1B = TCCR1B_INIT; // Select Timer1 prescale of CLK/1 7133 .LM1220: 7134 24c0 2EBD out 78-0x20,r18 2775:srx1.c **** 2776:srx1.c **** // Stop motors by default. 2777:srx1.c **** PWM_L = MAX_PWM_OUTPUT; 7136 .LM1221: 7137 24c2 8FE7 ldi r24,lo8(127) 7138 24c4 88BD out 72-0x20,r24 2778:srx1.c **** PWM_R = MAX_PWM_OUTPUT; 7140 .LM1222: 7141 24c6 8ABD out 74-0x20,r24 2779:srx1.c **** 2780:srx1.c **** // Configure wheel encoders ints. 2781:srx1.c **** //EIMSK = 0; // Disable interrupts just in case any interrupts are pending while changing 2782:srx1.c **** //EICRA = EICRA_INIT; // Configure default edge detection NOTE: Run this before unmasking the int 2783:srx1.c **** //EIMSK = EIMSK_INIT; // Enable ext. int #2-3 2784:srx1.c **** 2785:srx1.c **** // Enable and configure ADC channels (left adjusted) 2786:srx1.c **** InitADC(); 7143 .LM1223: 7144 24c8 0E94 0000 call InitADC 2787:srx1.c **** 2788:srx1.c **** // Initialize USART baud rate generator 2789:srx1.c **** InitSerialIO(UBRR_INIT); 7146 .LM1224: 7147 24cc 83E3 ldi r24,lo8(51) 7148 24ce 90E0 ldi r25,hi8(51) 7149 24d0 0E94 0000 call InitSerialIO 2790:srx1.c **** 2791:srx1.c **** // Debug to check if there was a reset! 2792:srx1.c **** UDR0 = '+'; 7151 .LM1225: 7152 24d4 8BE2 ldi r24,lo8(43) 7153 24d6 8CB9 out 44-0x20,r24 2793:srx1.c **** UDR0 = '+'; 7155 .LM1226: 7156 24d8 8CB9 out 44-0x20,r24 2794:srx1.c **** 2795:srx1.c **** // Initialize I2C interface 2796:srx1.c **** i2c_init(); 7158 .LM1227: 7159 24da 0E94 0000 call i2c_init 2797:srx1.c **** 2798:srx1.c **** // Initialise runtime parameter table 2799:srx1.c **** LoadParameterTable(); 7161 .LM1228: 7162 24de 0E94 0000 call LoadParameterTable 2800:srx1.c **** 2801:srx1.c **** // Initialise Behaviours 2802:srx1.c **** InitBehaviourTable(); 7164 .LM1229: 7165 24e2 0E94 0000 call InitBehaviourTable 2803:srx1.c **** 2804:srx1.c **** // Init SCM 2805:srx1.c **** SCM_Init(); 7167 .LM1230: 7168 24e6 0E94 0000 call SCM_Init 2806:srx1.c **** 2807:srx1.c **** // DEBUG: Init Character I/O for printf 2808:srx1.c **** //fdevopen((void *)PutChar, 0,0); 2809:srx1.c **** 2810:srx1.c **** // Start up primary set of tasks here. 2811:srx1.c **** AvrXRunTask(TCB(MotorTask)); 7170 .LM1231: 7171 24ea 80E0 ldi r24,lo8(MotorTaskTcb) 7172 24ec 90E0 ldi r25,hi8(MotorTaskTcb) 7173 24ee 0E94 0000 call AvrXRunTask 2812:srx1.c **** AvrXRunTask(TCB(exec)); 7175 .LM1232: 7176 24f2 80E0 ldi r24,lo8(execTcb) 7177 24f4 90E0 ldi r25,hi8(execTcb) 7178 24f6 0E94 0000 call AvrXRunTask 2813:srx1.c **** AvrXRunTask(TCB(arbitrate)); 7180 .LM1233: 7181 24fa 80E0 ldi r24,lo8(arbitrateTcb) 7182 24fc 90E0 ldi r25,hi8(arbitrateTcb) 7183 24fe 0E94 0000 call AvrXRunTask 2814:srx1.c **** AvrXRunTask(TCB(planner)); 7185 .LM1234: 7186 2502 80E0 ldi r24,lo8(plannerTcb) 7187 2504 90E0 ldi r25,hi8(plannerTcb) 7188 2506 0E94 0000 call AvrXRunTask 2815:srx1.c **** //AvrXRunTask(TCB(avoid)); //disabled temporarily for debugging purposes. Feb.25th 2816:srx1.c **** AvrXRunTask(TCB(follow)); 7190 .LM1235: 7191 250a 80E0 ldi r24,lo8(followTcb) 7192 250c 90E0 ldi r25,hi8(followTcb) 7193 250e 0E94 0000 call AvrXRunTask 2817:srx1.c **** AvrXRunTask(TCB(rotate)); 7195 .LM1236: 7196 2512 80E0 ldi r24,lo8(rotateTcb) 7197 2514 90E0 ldi r25,hi8(rotateTcb) 7198 2516 0E94 0000 call AvrXRunTask 2818:srx1.c **** AvrXRunTask(TCB(goat)); 7200 .LM1237: 7201 251a 80E0 ldi r24,lo8(goatTcb) 7202 251c 90E0 ldi r25,hi8(goatTcb) 7203 251e 0E94 0000 call AvrXRunTask 2819:srx1.c **** AvrXRunTask(TCB(forward)); 7205 .LM1238: 7206 2522 80E0 ldi r24,lo8(forwardTcb) 7207 2524 90E0 ldi r25,hi8(forwardTcb) 7208 2526 0E94 0000 call AvrXRunTask 2820:srx1.c **** AvrXRunTask(TCB(extinguish)); 7210 .LM1239: 7211 252a 80E0 ldi r24,lo8(extinguishTcb) 7212 252c 90E0 ldi r25,hi8(extinguishTcb) 7213 252e 0E94 0000 call AvrXRunTask 2821:srx1.c **** AvrXRunTask(TCB(search)); 7215 .LM1240: 7216 2532 80E0 ldi r24,lo8(searchTcb) 7217 2534 90E0 ldi r25,hi8(searchTcb) 7218 2536 0E94 0000 call AvrXRunTask 2822:srx1.c **** AvrXRunTask(TCB(align)); 7220 .LM1241: 7221 253a 80E0 ldi r24,lo8(alignTcb) 7222 253c 90E0 ldi r25,hi8(alignTcb) 7223 253e 0E94 0000 call AvrXRunTask 2823:srx1.c **** 2824:srx1.c **** Epilog(); // Switch from AvrX Stack to first task 7225 .LM1242: 7226 2542 0E94 0000 call Epilog 2825:srx1.c **** 2826:srx1.c **** return(0); // Just to get rid of the warnings! 2827:srx1.c **** } 7228 .LM1243: 7229 2546 80E0 ldi r24,lo8(0) 7230 2548 90E0 ldi r25,hi8(0) 7231 /* epilogue: frame size=0 */ 7232 254a 0C94 0000 jmp exit 7233 /* epilogue end (size=2) */ 7234 /* function main size 105 (99) */ 7236 .Lscope13: 7238 .lcomm AlignCmd.23,6 7241 .global Align 7243 Align: 2828:srx1.c **** 2829:srx1.c **** // 2830:srx1.c **** // Function Name: Align() 2831:srx1.c **** // Description: Aligns with Wall or Door frame. 2832:srx1.c **** // 2833:srx1.c **** unsigned char Align( unsigned char type ) 2834:srx1.c **** { 7245 .LM1244: 7246 /* prologue: frame size=0 */ 7247 /* prologue end (size=0) */ 2835:srx1.c **** static AlignMessage AlignCmd; 2836:srx1.c **** 2837:srx1.c **** AlignCmd.type = type; 7249 .LM1245: 7250 .LBB14: 7251 254e 8093 0000 sts AlignCmd.23+4,r24 2838:srx1.c **** AvrXSendMessage(&AlignQueue, &AlignCmd.mcb); 7253 .LM1246: 7254 2552 60E0 ldi r22,lo8(AlignCmd.23) 7255 2554 70E0 ldi r23,hi8(AlignCmd.23) 7256 2556 80E0 ldi r24,lo8(AlignQueue) 7257 2558 90E0 ldi r25,hi8(AlignQueue) 7258 255a 0E94 0000 call AvrXSendMessage 2839:srx1.c **** AvrXWaitMessageAck(&AlignCmd.mcb); 7260 .LM1247: 7261 255e 80E0 ldi r24,lo8(AlignCmd.23) 7262 2560 90E0 ldi r25,hi8(AlignCmd.23) 7263 2562 0E94 0000 call AvrXWaitMessageAck 2840:srx1.c **** 2841:srx1.c **** // sanity check. 2842:srx1.c **** if (AlignCmd.status != ALIGN_REPORT_SUCCESS) 7265 .LM1248: 7266 2566 8091 0000 lds r24,AlignCmd.23+5 7267 256a 8130 cpi r24,lo8(1) 7268 256c 91F0 breq .L477 2843:srx1.c **** { 2844:srx1.c **** PrintString(str_error); 7270 .LM1249: 7271 256e 80E0 ldi r24,lo8(str_error) 7272 2570 90E0 ldi r25,hi8(str_error) 7273 2572 0E94 0000 call PrintString 2845:srx1.c **** PutChar('A'); 7275 .LM1250: 7276 2576 81E4 ldi r24,lo8(65) 7277 2578 0E94 0000 call PutChar 2846:srx1.c **** PutDecByte(AlignCmd.status); 7279 .LM1251: 7280 257c 8091 0000 lds r24,AlignCmd.23+5 7281 2580 0E94 0000 call PutDecByte 2847:srx1.c **** // Stop motors 2848:srx1.c **** PWM_L = MAX_PWM_OUTPUT; 7283 .LM1252: 7284 2584 8FE7 ldi r24,lo8(127) 7285 2586 88BD out 72-0x20,r24 2849:srx1.c **** PWM_R = MAX_PWM_OUTPUT; 7287 .LM1253: 7288 2588 8ABD out 74-0x20,r24 2850:srx1.c **** AvrXSuspend(PID(planner)); 7290 .LM1254: 7291 258a 80E0 ldi r24,lo8(plannerPid) 7292 258c 90E0 ldi r25,hi8(plannerPid) 7293 258e 0E94 0000 call AvrXSuspend 7294 .L477: 2851:srx1.c **** } 2852:srx1.c **** 2853:srx1.c **** return (AlignCmd.status); 7296 .LM1255: 7297 2592 8091 0000 lds r24,AlignCmd.23+5 7298 2596 9927 clr r25 2854:srx1.c **** } 7300 .LM1256: 7301 .LBE14: 7302 /* epilogue: frame size=0 */ 7303 2598 0895 ret 7304 /* epilogue end (size=1) */ 7305 /* function Align size 38 (37) */ 7310 .Lscope14: 7312 .lcomm ForwardCmd.24,8 7316 .global Forward 7318 Forward: 2855:srx1.c **** 2856:srx1.c **** // 2857:srx1.c **** // Function Name: Forward() 2858:srx1.c **** // Description: Move Forward. 2859:srx1.c **** // 2860:srx1.c **** unsigned char Forward( unsigned char lenght, unsigned char expected) 2861:srx1.c **** { 7320 .LM1257: 7321 /* prologue: frame size=0 */ 7322 259a CF93 push r28 7323 /* prologue end (size=1) */ 7324 259c C62F mov r28,r22 2862:srx1.c **** static ForwardMessage ForwardCmd; 2863:srx1.c **** 2864:srx1.c **** ForwardCmd.lenght = lenght; 7326 .LM1258: 7327 .LBB15: 7328 259e 9927 clr r25 7329 25a0 9093 0000 sts (ForwardCmd.24+4)+1,r25 7330 25a4 8093 0000 sts ForwardCmd.24+4,r24 2865:srx1.c **** if (expected == FORWARD_REPORT_DOOR) 7332 .LM1259: 7333 25a8 6330 cpi r22,lo8(3) 7334 25aa 19F4 brne .L479 2866:srx1.c **** ForwardCmd.ignore_door = FALSE; 7336 .LM1260: 7337 25ac 1092 0000 sts ForwardCmd.24+6,__zero_reg__ 7338 25b0 03C0 rjmp .L480 7339 .L479: 2867:srx1.c **** else 2868:srx1.c **** ForwardCmd.ignore_door = TRUE; 7341 .LM1261: 7342 25b2 81E0 ldi r24,lo8(1) 7343 25b4 8093 0000 sts ForwardCmd.24+6,r24 7344 .L480: 2869:srx1.c **** 2870:srx1.c **** AvrXSendMessage(&ForwardQueue, &ForwardCmd.mcb); 7346 .LM1262: 7347 25b8 60E0 ldi r22,lo8(ForwardCmd.24) 7348 25ba 70E0 ldi r23,hi8(ForwardCmd.24) 7349 25bc 80E0 ldi r24,lo8(ForwardQueue) 7350 25be 90E0 ldi r25,hi8(ForwardQueue) 7351 25c0 0E94 0000 call AvrXSendMessage 2871:srx1.c **** AvrXWaitMessageAck(&ForwardCmd.mcb); 7353 .LM1263: 7354 25c4 80E0 ldi r24,lo8(ForwardCmd.24) 7355 25c6 90E0 ldi r25,hi8(ForwardCmd.24) 7356 25c8 0E94 0000 call AvrXWaitMessageAck 2872:srx1.c **** 2873:srx1.c **** // sanity check. 2874:srx1.c **** if (ForwardCmd.status != expected) 7358 .LM1264: 7359 25cc 8091 0000 lds r24,ForwardCmd.24+7 7360 25d0 8C17 cp r24,r28 7361 25d2 91F0 breq .L481 2875:srx1.c **** { 2876:srx1.c **** PrintString(str_error); 7363 .LM1265: 7364 25d4 80E0 ldi r24,lo8(str_error) 7365 25d6 90E0 ldi r25,hi8(str_error) 7366 25d8 0E94 0000 call PrintString 2877:srx1.c **** PutChar('F'); 7368 .LM1266: 7369 25dc 86E4 ldi r24,lo8(70) 7370 25de 0E94 0000 call PutChar 2878:srx1.c **** PutDecByte(ForwardCmd.status); 7372 .LM1267: 7373 25e2 8091 0000 lds r24,ForwardCmd.24+7 7374 25e6 0E94 0000 call PutDecByte 2879:srx1.c **** // Stop motors 2880:srx1.c **** PWM_L = MAX_PWM_OUTPUT; 7376 .LM1268: 7377 25ea 8FE7 ldi r24,lo8(127) 7378 25ec 88BD out 72-0x20,r24 2881:srx1.c **** PWM_R = MAX_PWM_OUTPUT; 7380 .LM1269: 7381 25ee 8ABD out 74-0x20,r24 2882:srx1.c **** AvrXSuspend(PID(planner)); 7383 .LM1270: 7384 25f0 80E0 ldi r24,lo8(plannerPid) 7385 25f2 90E0 ldi r25,hi8(plannerPid) 7386 25f4 0E94 0000 call AvrXSuspend 7387 .L481: 2883:srx1.c **** } 2884:srx1.c **** 2885:srx1.c **** return(ForwardCmd.status); 7389 .LM1271: 7390 25f8 8091 0000 lds r24,ForwardCmd.24+7 7391 25fc 9927 clr r25 2886:srx1.c **** } 7393 .LM1272: 7394 .LBE15: 7395 /* epilogue: frame size=0 */ 7396 25fe CF91 pop r28 7397 2600 0895 ret 7398 /* epilogue end (size=2) */ 7399 /* function Forward size 52 (49) */ 7404 .Lscope15: 7406 .lcomm RotateCmd.25,9 7410 .global Rotate 7412 Rotate: 2887:srx1.c **** 2888:srx1.c **** // Function Name: Rotate() 2889:srx1.c **** // Description: Rotate. 2890:srx1.c **** // 2891:srx1.c **** void Rotate( signed int angle, unsigned char scan ) 2892:srx1.c **** { 7414 .LM1273: 7415 /* prologue: frame size=0 */ 7416 2602 0F93 push r16 7417 2604 1F93 push r17 7418 /* prologue end (size=2) */ 2893:srx1.c **** static RotateMessage RotateCmd; 2894:srx1.c **** 2895:srx1.c **** RotateCmd.angle = angle; 7420 .LM1274: 7421 .LBB16: 7422 2606 00E0 ldi r16,lo8(RotateCmd.25+4) 7423 2608 10E0 ldi r17,hi8(RotateCmd.25+4) 7424 260a 9093 0000 sts (RotateCmd.25+4)+1,r25 7425 260e 8093 0000 sts RotateCmd.25+4,r24 2896:srx1.c **** RotateCmd.scan = scan; 7427 .LM1275: 7428 2612 6093 0000 sts RotateCmd.25+7,r22 2897:srx1.c **** AvrXSendMessage(&RotateQueue, &RotateCmd.mcb); 7430 .LM1276: 7431 2616 B801 movw r22,r16 7432 2618 6450 subi r22,lo8(-(-4)) 7433 261a 7040 sbci r23,hi8(-(-4)) 7434 261c 80E0 ldi r24,lo8(RotateQueue) 7435 261e 90E0 ldi r25,hi8(RotateQueue) 7436 2620 0E94 0000 call AvrXSendMessage 2898:srx1.c **** AvrXWaitMessageAck(&RotateCmd.mcb); 7438 .LM1277: 7439 2624 C801 movw r24,r16 7440 2626 0497 sbiw r24,4 7441 2628 0E94 0000 call AvrXWaitMessageAck 2899:srx1.c **** } 7443 .LM1278: 7444 .LBE16: 7445 /* epilogue: frame size=0 */ 7446 262c 1F91 pop r17 7447 262e 0F91 pop r16 7448 2630 0895 ret 7449 /* epilogue end (size=3) */ 7450 /* function Rotate size 24 (19) */ 7455 .Lscope16: 7457 .lcomm SearchCmd.26,6 7458 .lcomm ExtinguishCmd.27,5 7461 .global Search 7463 Search: 2900:srx1.c **** 2901:srx1.c **** // 2902:srx1.c **** // Function Name: Search() 2903:srx1.c **** // Description: Search. 2904:srx1.c **** // 2905:srx1.c **** unsigned char Search(unsigned char side) 2906:srx1.c **** { 7465 .LM1279: 7466 /* prologue: frame size=0 */ 7467 /* prologue end (size=0) */ 2907:srx1.c **** static SearchMessage SearchCmd; 2908:srx1.c **** static ExtinguishMessage ExtinguishCmd; 2909:srx1.c **** 2910:srx1.c **** SearchCmd.side = side; 7469 .LM1280: 7470 .LBB17: 7471 2632 8093 0000 sts SearchCmd.26+5,r24 2911:srx1.c **** AvrXSendMessage(&SearchQueue, &SearchCmd.mcb); 7473 .LM1281: 7474 2636 60E0 ldi r22,lo8(SearchCmd.26) 7475 2638 70E0 ldi r23,hi8(SearchCmd.26) 7476 263a 80E0 ldi r24,lo8(SearchQueue) 7477 263c 90E0 ldi r25,hi8(SearchQueue) 7478 263e 0E94 0000 call AvrXSendMessage 2912:srx1.c **** AvrXWaitMessageAck(&SearchCmd.mcb); 7480 .LM1282: 7481 2642 80E0 ldi r24,lo8(SearchCmd.26) 7482 2644 90E0 ldi r25,hi8(SearchCmd.26) 7483 2646 0E94 0000 call AvrXWaitMessageAck 2913:srx1.c **** 2914:srx1.c **** // If candle was found, proceed to extinguish it. 2915:srx1.c **** if (SearchCmd.status == TRUE) 7485 .LM1283: 7486 264a 8091 0000 lds r24,SearchCmd.26+4 7487 264e 8130 cpi r24,lo8(1) 7488 2650 69F4 brne .L484 2916:srx1.c **** { 2917:srx1.c **** AvrXSendMessage(&ExtinguishQueue, &ExtinguishCmd.mcb); 7490 .LM1284: 7491 2652 60E0 ldi r22,lo8(ExtinguishCmd.27) 7492 2654 70E0 ldi r23,hi8(ExtinguishCmd.27) 7493 2656 80E0 ldi r24,lo8(ExtinguishQueue) 7494 2658 90E0 ldi r25,hi8(ExtinguishQueue) 7495 265a 0E94 0000 call AvrXSendMessage 2918:srx1.c **** AvrXWaitMessageAck(&ExtinguishCmd.mcb); 7497 .LM1285: 7498 265e 80E0 ldi r24,lo8(ExtinguishCmd.27) 7499 2660 90E0 ldi r25,hi8(ExtinguishCmd.27) 7500 2662 0E94 0000 call AvrXWaitMessageAck 2919:srx1.c **** return(TRUE); 7502 .LM1286: 7503 2666 81E0 ldi r24,lo8(1) 7504 2668 90E0 ldi r25,hi8(1) 2920:srx1.c **** } 2921:srx1.c **** else 2922:srx1.c **** return(FALSE); 2923:srx1.c **** } 7506 .LM1287: 7507 266a 0895 ret 7508 .L484: 7510 .LM1288: 7511 266c 80E0 ldi r24,lo8(0) 7512 266e 90E0 ldi r25,hi8(0) 7514 .LM1289: 7515 2670 0895 ret 7516 .LBE17: 7517 /* epilogue: frame size=0 */ 7518 /* epilogue: noreturn */ 7519 /* epilogue end (size=0) */ 7520 /* function Search size 32 (32) */ 7526 .Lscope17: 7528 .lcomm FollowCmd.28,7 7532 .global Follow 7534 Follow: 2924:srx1.c **** 2925:srx1.c **** // 2926:srx1.c **** // Function Name: Follow() 2927:srx1.c **** // Description: Follow wall. 2928:srx1.c **** // 2929:srx1.c **** unsigned char Follow(unsigned char side, unsigned char expected) 2930:srx1.c **** { 7536 .LM1290: 7537 /* prologue: frame size=0 */ 7538 2672 CF93 push r28 7539 /* prologue end (size=1) */ 7540 2674 C62F mov r28,r22 2931:srx1.c **** static FollowMessage FollowCmd; 2932:srx1.c **** 2933:srx1.c **** FollowCmd.side = side; 7542 .LM1291: 7543 .LBB18: 7544 2676 8093 0000 sts FollowCmd.28+4,r24 2934:srx1.c **** if (expected == FOLLOW_REPORT_DOOR) 7546 .LM1292: 7547 267a 6330 cpi r22,lo8(3) 7548 267c 19F4 brne .L487 2935:srx1.c **** FollowCmd.ignore_door = FALSE; 7550 .LM1293: 7551 267e 1092 0000 sts FollowCmd.28+5,__zero_reg__ 7552 2682 03C0 rjmp .L488 7553 .L487: 2936:srx1.c **** else 2937:srx1.c **** FollowCmd.ignore_door = TRUE; 7555 .LM1294: 7556 2684 81E0 ldi r24,lo8(1) 7557 2686 8093 0000 sts FollowCmd.28+5,r24 7558 .L488: 2938:srx1.c **** 2939:srx1.c **** AvrXSendMessage(&FollowQueue, &FollowCmd.mcb); 7560 .LM1295: 7561 268a 60E0 ldi r22,lo8(FollowCmd.28) 7562 268c 70E0 ldi r23,hi8(FollowCmd.28) 7563 268e 80E0 ldi r24,lo8(FollowQueue) 7564 2690 90E0 ldi r25,hi8(FollowQueue) 7565 2692 0E94 0000 call AvrXSendMessage 2940:srx1.c **** AvrXWaitMessageAck(&FollowCmd.mcb); 7567 .LM1296: 7568 2696 80E0 ldi r24,lo8(FollowCmd.28) 7569 2698 90E0 ldi r25,hi8(FollowCmd.28) 7570 269a 0E94 0000 call AvrXWaitMessageAck 2941:srx1.c **** 2942:srx1.c **** // sanity check. 2943:srx1.c **** if (FollowCmd.status != expected) 7572 .LM1297: 7573 269e 8091 0000 lds r24,FollowCmd.28+6 7574 26a2 8C17 cp r24,r28 7575 26a4 91F0 breq .L489 2944:srx1.c **** { 2945:srx1.c **** PrintString(str_error); 7577 .LM1298: 7578 26a6 80E0 ldi r24,lo8(str_error) 7579 26a8 90E0 ldi r25,hi8(str_error) 7580 26aa 0E94 0000 call PrintString 2946:srx1.c **** PutChar('T'); 7582 .LM1299: 7583 26ae 84E5 ldi r24,lo8(84) 7584 26b0 0E94 0000 call PutChar 2947:srx1.c **** PutDecByte(FollowCmd.status); 7586 .LM1300: 7587 26b4 8091 0000 lds r24,FollowCmd.28+6 7588 26b8 0E94 0000 call PutDecByte 2948:srx1.c **** // Stop motors 2949:srx1.c **** PWM_L = MAX_PWM_OUTPUT; 7590 .LM1301: 7591 26bc 8FE7 ldi r24,lo8(127) 7592 26be 88BD out 72-0x20,r24 2950:srx1.c **** PWM_R = MAX_PWM_OUTPUT; 7594 .LM1302: 7595 26c0 8ABD out 74-0x20,r24 2951:srx1.c **** AvrXSuspend(PID(planner)); 7597 .LM1303: 7598 26c2 80E0 ldi r24,lo8(plannerPid) 7599 26c4 90E0 ldi r25,hi8(plannerPid) 7600 26c6 0E94 0000 call AvrXSuspend 7601 .L489: 2952:srx1.c **** } 2953:srx1.c **** 2954:srx1.c **** return (FollowCmd.status); 7603 .LM1304: 7604 26ca 8091 0000 lds r24,FollowCmd.28+6 7605 26ce 9927 clr r25 2955:srx1.c **** } 7607 .LM1305: 7608 .LBE18: 7609 /* epilogue: frame size=0 */ 7610 26d0 CF91 pop r28 7611 26d2 0895 ret 7612 /* epilogue end (size=2) */ 7613 /* function Follow size 49 (46) */ 7618 .Lscope18: 7620 .lcomm GoAtCmd.29,10 7621 .section .progmem.data 7624 __c.30: 7625 0634 496C 6C65 .string "Illegal Room #" 7625 6761 6C20 7625 526F 6F6D 7625 2023 00 7626 .text 7628 .global GoHome 7630 GoHome: 2956:srx1.c **** 2957:srx1.c **** // 2958:srx1.c **** // Function Name: GoHome() 2959:srx1.c **** // Description: Return to Home position. 2960:srx1.c **** // 2961:srx1.c **** void GoHome(void) 2962:srx1.c **** { 7632 .LM1306: 7633 /* prologue: frame size=0 */ 7634 26d4 CF93 push r28 7635 /* prologue end (size=1) */ 2963:srx1.c **** static GoAtMessage GoAtCmd; 2964:srx1.c **** 2965:srx1.c **** // In all cases we need to return back to the door. 2966:srx1.c **** GoAtCmd.x = 0; 7637 .LM1307: 7638 .LBB19: 7639 26d6 1092 0000 sts (GoAtCmd.29+4)+1,__zero_reg__ 7640 26da 1092 0000 sts GoAtCmd.29+4,__zero_reg__ 2967:srx1.c **** GoAtCmd.y = 0; 7642 .LM1308: 7643 26de 1092 0000 sts (GoAtCmd.29+6)+1,__zero_reg__ 7644 26e2 1092 0000 sts GoAtCmd.29+6,__zero_reg__ 2968:srx1.c **** GoAtCmd.theta = 180; // Face in opposing direction (towards door) 7646 .LM1309: 7647 26e6 84EB ldi r24,lo8(180) 7648 26e8 90E0 ldi r25,hi8(180) 7649 26ea 9093 0000 sts (GoAtCmd.29+8)+1,r25 7650 26ee 8093 0000 sts GoAtCmd.29+8,r24 2969:srx1.c **** AvrXSendMessage(&GoAtQueue, &GoAtCmd.mcb); 7652 .LM1310: 7653 26f2 60E0 ldi r22,lo8(GoAtCmd.29) 7654 26f4 70E0 ldi r23,hi8(GoAtCmd.29) 7655 26f6 80E0 ldi r24,lo8(GoAtQueue) 7656 26f8 90E0 ldi r25,hi8(GoAtQueue) 7657 26fa 0E94 0000 call AvrXSendMessage 2970:srx1.c **** AvrXWaitMessageAck(&GoAtCmd.mcb); 7659 .LM1311: 7660 26fe 80E0 ldi r24,lo8(GoAtCmd.29) 7661 2700 90E0 ldi r25,hi8(GoAtCmd.29) 7662 2702 0E94 0000 call AvrXWaitMessageAck 2971:srx1.c **** 2972:srx1.c **** if (Room == 1 || Room == 2) 7664 .LM1312: 7665 2706 C091 0000 lds r28,Room 7666 270a 8C2F mov r24,r28 7667 270c 8150 subi r24,lo8(-(-1)) 7668 270e 8230 cpi r24,lo8(2) 7669 2710 E0F5 brsh .L491 2973:srx1.c **** { 2974:srx1.c **** if (Room == 1) // Room1 specific path 7671 .LM1313: 7672 2712 C130 cpi r28,lo8(1) 7673 2714 A1F4 brne .L492 2975:srx1.c **** { 2976:srx1.c **** // Align with Right Wall 2977:srx1.c **** Align(ALIGN_RIGHT); 7675 .LM1314: 7676 2716 8C2F mov r24,r28 7677 2718 0E94 0000 call Align 2978:srx1.c **** 2979:srx1.c **** // And go forward until door is found. 2980:srx1.c **** Forward(0, FORWARD_REPORT_DOOR); 7679 .LM1315: 7680 271c 63E0 ldi r22,lo8(3) 7681 271e 80E0 ldi r24,lo8(0) 7682 2720 0E94 0000 call Forward 2981:srx1.c **** 2982:srx1.c **** // Align with Door frame 2983:srx1.c **** //Align(ALIGN_DOOR); 2984:srx1.c **** 2985:srx1.c **** // Move forward to clear door frame 2986:srx1.c **** //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); 2987:srx1.c **** 2988:srx1.c **** // Move forward 'till wall 2989:srx1.c **** Forward(0, FORWARD_REPORT_OBSTACLE); 7684 .LM1316: 7685 2724 62E0 ldi r22,lo8(2) 7686 2726 80E0 ldi r24,lo8(0) 7687 2728 0E94 0000 call Forward 2990:srx1.c **** 2991:srx1.c **** // Rotate Right 90 degrees 2992:srx1.c **** Rotate(90, FALSE); 7689 .LM1317: 7690 272c 60E0 ldi r22,lo8(0) 7691 272e 8AE5 ldi r24,lo8(90) 7692 2730 90E0 ldi r25,hi8(90) 7693 2732 0E94 0000 call Rotate 2993:srx1.c **** 2994:srx1.c **** // Align with Left Wall 2995:srx1.c **** Align(ALIGN_LEFT); 7695 .LM1318: 7696 2736 80E0 ldi r24,lo8(0) 7697 2738 0E94 0000 call Align 7698 273c 16C0 rjmp .L493 7699 .L492: 2996:srx1.c **** } 2997:srx1.c **** else 2998:srx1.c **** if (Room == 2) // Room2 specific path 7701 .LM1319: 7702 273e C230 cpi r28,lo8(2) 7703 2740 A1F4 brne .L493 2999:srx1.c **** { 3000:srx1.c **** // Exit room by following Right wall. 3001:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_DOOR); 7705 .LM1320: 7706 2742 63E0 ldi r22,lo8(3) 7707 2744 81E0 ldi r24,lo8(1) 7708 2746 0E94 0000 call Follow 3002:srx1.c **** 3003:srx1.c **** // Align with Door frame (and clear it) 3004:srx1.c **** //Align(ALIGN_DOOR); 3005:srx1.c **** 3006:srx1.c **** // Move forward to clear door frame 3007:srx1.c **** //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); 3008:srx1.c **** 3009:srx1.c **** // Keep following right wall 'till obstacle 3010:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_OBSTACLE); 7710 .LM1321: 7711 274a 6C2F mov r22,r28 7712 274c 81E0 ldi r24,lo8(1) 7713 274e 0E94 0000 call Follow 3011:srx1.c **** 3012:srx1.c **** // Rotate Left 90 degrees 3013:srx1.c **** Rotate(-90, FALSE); 7715 .LM1322: 7716 2752 60E0 ldi r22,lo8(0) 7717 2754 86EA ldi r24,lo8(-90) 7718 2756 9FEF ldi r25,hi8(-90) 7719 2758 0E94 0000 call Rotate 3014:srx1.c **** 3015:srx1.c **** // Align with Right wall 3016:srx1.c **** Align(ALIGN_RIGHT); 7721 .LM1323: 7722 275c 81E0 ldi r24,lo8(1) 7723 275e 0E94 0000 call Align 3017:srx1.c **** 3018:srx1.c **** // Go forward a fixed amount so we can find left wall 3019:srx1.c **** Forward(47, FORWARD_REPORT_SUCCESS); 7725 .LM1324: 7726 2762 61E0 ldi r22,lo8(1) 7727 2764 8FE2 ldi r24,lo8(47) 7728 2766 0E94 0000 call Forward 7729 .L493: 3020:srx1.c **** } 3021:srx1.c **** 3022:srx1.c **** // Common path 3023:srx1.c **** Forward(48, FORWARD_REPORT_SUCCESS); // TODO: make more robust (no hardcoded distances) 7731 .LM1325: 7732 276a 61E0 ldi r22,lo8(1) 7733 276c 80E3 ldi r24,lo8(48) 7734 276e 0E94 0000 call Forward 3024:srx1.c **** Rotate(90, FALSE); 7736 .LM1326: 7737 2772 60E0 ldi r22,lo8(0) 7738 2774 8AE5 ldi r24,lo8(90) 7739 2776 90E0 ldi r25,hi8(90) 7740 2778 0E94 0000 call Rotate 3025:srx1.c **** Forward(51, FORWARD_REPORT_SUCCESS); // TODO: make more robust (no hardcoded distances) 7742 .LM1327: 7743 277c 61E0 ldi r22,lo8(1) 7744 277e 83E3 ldi r24,lo8(51) 7745 2780 0E94 0000 call Forward 3026:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_DOOR); // Until Home circle is sensed. 7747 .LM1328: 7748 2784 63E0 ldi r22,lo8(3) 7749 2786 81E0 ldi r24,lo8(1) 7750 2788 57C0 rjmp .L502 7751 .L491: 3027:srx1.c **** } 3028:srx1.c **** else 3029:srx1.c **** if (Room == 3 || Room == 4) 7753 .LM1329: 7754 278a 8C2F mov r24,r28 7755 278c 8350 subi r24,lo8(-(-3)) 7756 278e 8230 cpi r24,lo8(2) 7757 2790 08F0 brlo .+2 7758 2792 55C0 rjmp .L496 3030:srx1.c **** { 3031:srx1.c **** if (Room == 3) // Room3 specific path 7760 .LM1330: 7761 2794 C330 cpi r28,lo8(3) 7762 2796 A1F5 brne .L497 3032:srx1.c **** { 3033:srx1.c **** // Exit room by following Right wall. 3034:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_DOOR); 7764 .LM1331: 7765 2798 6C2F mov r22,r28 7766 279a 81E0 ldi r24,lo8(1) 7767 279c 0E94 0000 call Follow 3035:srx1.c **** 3036:srx1.c **** // Align with Door frame (and clear it) 3037:srx1.c **** //Align(ALIGN_DOOR); 3038:srx1.c **** 3039:srx1.c **** // Move forward to clear door frame 3040:srx1.c **** //Forward(DOOR_CLEAR_DISTANCE, FORWARD_REPORT_SUCCESS); 3041:srx1.c **** 3042:srx1.c **** // Keep following right wall 'till obstacle 3043:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_OBSTACLE); 7769 .LM1332: 7770 27a0 62E0 ldi r22,lo8(2) 7771 27a2 81E0 ldi r24,lo8(1) 7772 27a4 0E94 0000 call Follow 3044:srx1.c **** 3045:srx1.c **** // Rotate Left 90 degrees 3046:srx1.c **** Rotate(-90, FALSE); 7774 .LM1333: 7775 27a8 60E0 ldi r22,lo8(0) 7776 27aa 86EA ldi r24,lo8(-90) 7777 27ac 9FEF ldi r25,hi8(-90) 7778 27ae 0E94 0000 call Rotate 3047:srx1.c **** 3048:srx1.c **** // Align with Right wall 3049:srx1.c **** Align(ALIGN_RIGHT); 7780 .LM1334: 7781 27b2 81E0 ldi r24,lo8(1) 7782 27b4 0E94 0000 call Align 3050:srx1.c **** 3051:srx1.c **** // Follow right wall 'till the end 3052:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_SUCCESS); 7784 .LM1335: 7785 27b8 61E0 ldi r22,lo8(1) 7786 27ba 862F mov r24,r22 7787 27bc 0E94 0000 call Follow 3053:srx1.c **** 3054:srx1.c **** // Move forward 'till obstacle detected 3055:srx1.c **** Forward(0, FORWARD_REPORT_OBSTACLE); 7789 .LM1336: 7790 27c0 62E0 ldi r22,lo8(2) 7791 27c2 80E0 ldi r24,lo8(0) 7792 27c4 0E94 0000 call Forward 3056:srx1.c **** 3057:srx1.c **** // Rotate Left 90 degrees 3058:srx1.c **** Rotate(-90, FALSE); 7794 .LM1337: 7795 27c8 60E0 ldi r22,lo8(0) 7796 27ca 86EA ldi r24,lo8(-90) 7797 27cc 9FEF ldi r25,hi8(-90) 7798 27ce 0E94 0000 call Rotate 3059:srx1.c **** 3060:srx1.c **** // Align with Right wall 3061:srx1.c **** Align(ALIGN_RIGHT); 7800 .LM1338: 7801 27d2 81E0 ldi r24,lo8(1) 7802 27d4 0E94 0000 call Align 3062:srx1.c **** 3063:srx1.c **** // Follow wall for a little bit 3064:srx1.c **** Follow(RIGHT, FOLLOW_REPORT_SUCCESS); 7804 .LM1339: 7805 27d8 61E0 ldi r22,lo8(1) 7806 27da 862F mov r24,r22 7807 27dc 0E94 0000 call Follow 3065:srx1.c **** 3066:srx1.c **** // Move forward to clear intersection 3067:srx1.c **** Forward(0, FORWARD_REPORT_OBSTACLE); 7809 .LM1340: 7810 27e0 62E0 ldi r22,lo8(2) 7811 27e2 80E0 ldi r24,lo8(0) 7812 27e4 0E94 0000 call Forward 3068:srx1.c **** 3069:srx1.c **** // Align with Right wall 3070:srx1.c **** Align(ALIGN_LEFT); 7814 .LM1341: 7815 27e8 80E0 ldi r24,lo8(0) 7816 27ea 0E94 0000 call Align 3071:srx1.c **** 3072:srx1.c **** // Rotate Right 90 degrees 3073:srx1.c **** Rotate(+90, FALSE); 7818 .LM1342: 7819 27ee 60E0 ldi r22,lo8(0) 7820 27f0 8AE5 ldi r24,lo8(90) 7821 27f2 90E0 ldi r25,hi8(90) 7822 27f4 0E94 0000 call Rotate 3074:srx1.c **** 3075:srx1.c **** // Align with Left wall 3076:srx1.c **** Align(ALIGN_LEFT); 7824 .LM1343: 7825 27f8 80E0 ldi r24,lo8(0) 7826 27fa 0E94 0000 call Align 3077:srx1.c **** 3078:srx1.c **** // Follow Left wall 'till obstacle detected. 3079:srx1.c **** Follow(LEFT, FOLLOW_REPORT_OBSTACLE); 7828 .LM1344: 7829 27fe 0EC0 rjmp .L501 7830 .L497: 3080:srx1.c **** } 3081:srx1.c **** else 3082:srx1.c **** if (Room == 4) 7832 .LM1345: 7833 2800 C430 cpi r28,lo8(4) 7834 2802 81F4 brne .L498 3083:srx1.c **** { 3084:srx1.c **** // Align with left wall 3085:srx1.c **** Align(ALIGN_LEFT); 7836 .LM1346: 7837 2804 80E0 ldi r24,lo8(0) 7838 2806 0E94 0000 call Align 3086:srx1.c **** 3087:srx1.c **** // Move forward 'till obstacle detected 3088:srx1.c **** Forward(0, FORWARD_REPORT_OBSTACLE); 7840 .LM1347: 7841 280a 62E0 ldi r22,lo8(2) 7842 280c 80E0 ldi r24,lo8(0) 7843 280e 0E94 0000 call Forward 3089:srx1.c **** 3090:srx1.c **** // Rotate Right 90 degrees 3091:srx1.c **** Rotate(+90, FALSE); 7845 .LM1348: 7846 2812 60E0 ldi r22,lo8(0) 7847 2814 8AE5 ldi r24,lo8(90) 7848 2816 90E0 ldi r25,hi8(90) 7849 2818 0E94 0000 call Rotate 7850 .L501: 3092:srx1.c **** 3093:srx1.c **** // Follow Left wall 'till obstacle detected. 3094:srx1.c **** Follow(LEFT, FOLLOW_REPORT_OBSTACLE); 7852 .LM1349: 7853 281c 62E0 ldi r22,lo8(2) 7854 281e 80E0 ldi r24,lo8(0) 7855 2820 0E94 0000 call Follow 7856 .L498: 3095:srx1.c **** } 3096:srx1.c **** 3097:srx1.c **** // Common path 3098:srx1.c **** Align(ALIGN_LEFT); 7858 .LM1350: 7859 2824 80E0 ldi r24,lo8(0) 7860 2826 0E94 0000 call Align 3099:srx1.c **** Rotate(+90, FALSE); 7862 .LM1351: 7863 282a 60E0 ldi r22,lo8(0) 7864 282c 8AE5 ldi r24,lo8(90) 7865 282e 90E0 ldi r25,hi8(90) 7866 2830 0E94 0000 call Rotate 3100:srx1.c **** Follow(LEFT, FOLLOW_REPORT_DOOR); 7868 .LM1352: 7869 2834 63E0 ldi r22,lo8(3) 7870 2836 80E0 ldi r24,lo8(0) 7871 .L502: 7872 2838 0E94 0000 call Follow 7873 283c 0AC0 rjmp .L495 7874 .L496: 3101:srx1.c **** } 3102:srx1.c **** else // Something is not right! 3103:srx1.c **** { 3104:srx1.c **** PrintString(str_error); 7876 .LM1353: 7877 283e 80E0 ldi r24,lo8(str_error) 7878 2840 90E0 ldi r25,hi8(str_error) 7879 2842 0E94 0000 call PrintString 3105:srx1.c **** PrintString(PSTR("Illegal Room #")); 7881 .LM1354: 7882 2846 80E0 ldi r24,lo8(__c.30) 7883 2848 90E0 ldi r25,hi8(__c.30) 7884 284a 0E94 0000 call PrintString 3106:srx1.c **** AvrXHalt(); 7886 .LM1355: 7887 284e 0E94 0000 call AvrXHalt 7888 .L495: 3107:srx1.c **** } 3108:srx1.c **** 3109:srx1.c **** // TODO: Issue a final stop command and halt everything. 3110:srx1.c **** AvrXHalt(); 7890 .LM1356: 7891 2852 0E94 0000 call AvrXHalt 3111:srx1.c **** } 7893 .LM1357: 7894 .LBE19: 7895 /* epilogue: frame size=0 */ 7896 2856 CF91 pop r28 7897 2858 0895 ret 7898 /* epilogue end (size=2) */ 7899 /* function GoHome size 195 (192) */ 7904 .Lscope19: 7907 .global LoadParameterTable 7909 LoadParameterTable: 3112:srx1.c **** 3113:srx1.c **** 3114:srx1.c **** // 3115:srx1.c **** // Function name: LoadParameterTable() 3116:srx1.c **** // Description: Reload the run-time paramenter table from EEPROM. 3117:srx1.c **** // 3118:srx1.c **** void LoadParameterTable() 3119:srx1.c **** { 7911 .LM1358: 7912 /* prologue: frame size=0 */ 7913 /* prologue end (size=0) */ 3120:srx1.c **** eeprom_read_block((ConfigData*)&R_ParameterTable, (ConfigData *)&E_ParameterTable, sizeof(ConfigDa 7915 .LM1359: 7916 285a 4BE0 ldi r20,lo8(11) 7917 285c 50E0 ldi r21,hi8(11) 7918 285e 60E0 ldi r22,lo8(E_ParameterTable) 7919 2860 70E0 ldi r23,hi8(E_ParameterTable) 7920 2862 80E0 ldi r24,lo8(R_ParameterTable) 7921 2864 90E0 ldi r25,hi8(R_ParameterTable) 7922 2866 0E94 0000 call eeprom_read_block 3121:srx1.c **** } 7924 .LM1360: 7925 /* epilogue: frame size=0 */ 7926 286a 0895 ret 7927 /* epilogue end (size=1) */ 7928 /* function LoadParameterTable size 9 (8) */ 7930 .Lscope20: 7933 .global InitParameterTable 7935 InitParameterTable: 3122:srx1.c **** 3123:srx1.c **** // 3124:srx1.c **** // Function name: InitParameterTable() 3125:srx1.c **** // Description: Re-Initialises the EEPROM paramenter table (when the ParameterTable changes) 3126:srx1.c **** // 3127:srx1.c **** void InitParameterTable() 3128:srx1.c **** { 7937 .LM1361: 7938 /* prologue: frame size=0 */ 7939 /* prologue end (size=0) */ 3129:srx1.c **** eeprom_write_byte(&E_ParameterTable.f_p, F_P); 7941 .LM1362: 7942 286c 64E1 ldi r22,lo8(20) 7943 286e 80E0 ldi r24,lo8(E_ParameterTable+1) 7944 2870 90E0 ldi r25,hi8(E_ParameterTable+1) 7945 2872 0E94 0000 call eeprom_write_byte 3130:srx1.c **** eeprom_write_byte(&E_ParameterTable.f_d, F_D); 7947 .LM1363: 7948 2876 6AE0 ldi r22,lo8(10) 7949 2878 80E0 ldi r24,lo8(E_ParameterTable+2) 7950 287a 90E0 ldi r25,hi8(E_ParameterTable+2) 7951 287c 0E94 0000 call eeprom_write_byte 3131:srx1.c **** eeprom_write_byte(&E_ParameterTable.Kp, K_P); 7953 .LM1364: 7954 2880 6BE1 ldi r22,lo8(27) 7955 2882 80E0 ldi r24,lo8(E_ParameterTable+3) 7956 2884 90E0 ldi r25,hi8(E_ParameterTable+3) 7957 2886 0E94 0000 call eeprom_write_byte 3132:srx1.c **** eeprom_write_byte(&E_ParameterTable.Ki, K_I); 7959 .LM1365: 7960 288a 60E0 ldi r22,lo8(0) 7961 288c 80E0 ldi r24,lo8(E_ParameterTable+4) 7962 288e 90E0 ldi r25,hi8(E_ParameterTable+4) 7963 2890 0E94 0000 call eeprom_write_byte 3133:srx1.c **** eeprom_write_byte(&E_ParameterTable.Kd, K_D); 7965 .LM1366: 7966 2894 61E2 ldi r22,lo8(33) 7967 2896 80E0 ldi r24,lo8(E_ParameterTable+5) 7968 2898 90E0 ldi r25,hi8(E_ParameterTable+5) 7969 289a 0E94 0000 call eeprom_write_byte 3134:srx1.c **** eeprom_write_byte(&E_ParameterTable.line_threshold, LINE_SENSOR_THRESHOLD); 7971 .LM1367: 7972 289e 6FE8 ldi r22,lo8(-113) 7973 28a0 80E0 ldi r24,lo8(E_ParameterTable+6) 7974 28a2 90E0 ldi r25,hi8(E_ParameterTable+6) 7975 28a4 0E94 0000 call eeprom_write_byte 3135:srx1.c **** eeprom_write_byte(&E_ParameterTable.scm_exposure_h, SCM_DEF_EXPOSURE_H); 7977 .LM1368: 7978 28a8 66E0 ldi r22,lo8(6) 7979 28aa 80E0 ldi r24,lo8(E_ParameterTable+7) 7980 28ac 90E0 ldi r25,hi8(E_ParameterTable+7) 7981 28ae 0E94 0000 call eeprom_write_byte 3136:srx1.c **** eeprom_write_byte(&E_ParameterTable.scm_exposure_l, SCM_DEF_EXPOSURE_L); 7983 .LM1369: 7984 28b2 60E0 ldi r22,lo8(0) 7985 28b4 80E0 ldi r24,lo8(E_ParameterTable+8) 7986 28b6 90E0 ldi r25,hi8(E_ParameterTable+8) 7987 28b8 0E94 0000 call eeprom_write_byte 3137:srx1.c **** eeprom_write_byte(&E_ParameterTable.scm_threshold, SCM_DEF_THRESHOLD); 7989 .LM1370: 7990 28bc 66EE ldi r22,lo8(-26) 7991 28be 80E0 ldi r24,lo8(E_ParameterTable+9) 7992 28c0 90E0 ldi r25,hi8(E_ParameterTable+9) 7993 28c2 0E94 0000 call eeprom_write_byte 3138:srx1.c **** eeprom_write_byte(&E_ParameterTable.scm_min_size, SCM_DEF_MIN_SIZE); 7995 .LM1371: 7996 28c6 64E0 ldi r22,lo8(4) 7997 28c8 80E0 ldi r24,lo8(E_ParameterTable+10) 7998 28ca 90E0 ldi r25,hi8(E_ParameterTable+10) 7999 28cc 0E94 0000 call eeprom_write_byte 3139:srx1.c **** } 8001 .LM1372: 8002 /* epilogue: frame size=0 */ 8003 28d0 0895 ret 8004 /* epilogue end (size=1) */ 8005 /* function InitParameterTable size 51 (50) */ 8007 .Lscope21: 8010 .global DumpParameterTable 8012 DumpParameterTable: 3140:srx1.c **** 3141:srx1.c **** // 3142:srx1.c **** // Function name: DumpParameterTable() 3143:srx1.c **** // Description: Dumps the content of the EEPROM paramenter table. 3144:srx1.c **** // 3145:srx1.c **** void DumpParameterTable(void) 3146:srx1.c **** { 8014 .LM1373: 8015 /* prologue: frame size=0 */ 8016 /* prologue end (size=0) */ 3147:srx1.c **** PrintString(str_parameter_header); 8018 .LM1374: 8019 28d2 80E0 ldi r24,lo8(str_parameter_header) 8020 28d4 90E0 ldi r25,hi8(str_parameter_header) 8021 28d6 0E94 0000 call PrintString 3148:srx1.c **** 3149:srx1.c **** PrintString(str_parameter_f_p); 8023 .LM1375: 8024 28da 80E0 ldi r24,lo8(str_parameter_f_p) 8025 28dc 90E0 ldi r25,hi8(str_parameter_f_p) 8026 28de 0E94 0000 call PrintString 3150:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.f_p)); 8028 .LM1376: 8029 28e2 80E0 ldi r24,lo8(E_ParameterTable+1) 8030 28e4 90E0 ldi r25,hi8(E_ParameterTable+1) 8031 28e6 0E94 0000 call eeprom_read_byte 8032 28ea 0E94 0000 call PutDecByte 3151:srx1.c **** 3152:srx1.c **** PrintString(str_parameter_f_d); 8034 .LM1377: 8035 28ee 80E0 ldi r24,lo8(str_parameter_f_d) 8036 28f0 90E0 ldi r25,hi8(str_parameter_f_d) 8037 28f2 0E94 0000 call PrintString 3153:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.f_d)); 8039 .LM1378: 8040 28f6 80E0 ldi r24,lo8(E_ParameterTable+2) 8041 28f8 90E0 ldi r25,hi8(E_ParameterTable+2) 8042 28fa 0E94 0000 call eeprom_read_byte 8043 28fe 0E94 0000 call PutDecByte 3154:srx1.c **** 3155:srx1.c **** PrintString(str_parameter_Kp); 8045 .LM1379: 8046 2902 80E0 ldi r24,lo8(str_parameter_Kp) 8047 2904 90E0 ldi r25,hi8(str_parameter_Kp) 8048 2906 0E94 0000 call PrintString 3156:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.Kp)); 8050 .LM1380: 8051 290a 80E0 ldi r24,lo8(E_ParameterTable+3) 8052 290c 90E0 ldi r25,hi8(E_ParameterTable+3) 8053 290e 0E94 0000 call eeprom_read_byte 8054 2912 0E94 0000 call PutDecByte 3157:srx1.c **** 3158:srx1.c **** PrintString(str_parameter_Ki); 8056 .LM1381: 8057 2916 80E0 ldi r24,lo8(str_parameter_Ki) 8058 2918 90E0 ldi r25,hi8(str_parameter_Ki) 8059 291a 0E94 0000 call PrintString 3159:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.Ki)); 8061 .LM1382: 8062 291e 80E0 ldi r24,lo8(E_ParameterTable+4) 8063 2920 90E0 ldi r25,hi8(E_ParameterTable+4) 8064 2922 0E94 0000 call eeprom_read_byte 8065 2926 0E94 0000 call PutDecByte 3160:srx1.c **** 3161:srx1.c **** PrintString(str_parameter_Kd); 8067 .LM1383: 8068 292a 80E0 ldi r24,lo8(str_parameter_Kd) 8069 292c 90E0 ldi r25,hi8(str_parameter_Kd) 8070 292e 0E94 0000 call PrintString 3162:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.Kd)); 8072 .LM1384: 8073 2932 80E0 ldi r24,lo8(E_ParameterTable+5) 8074 2934 90E0 ldi r25,hi8(E_ParameterTable+5) 8075 2936 0E94 0000 call eeprom_read_byte 8076 293a 0E94 0000 call PutDecByte 3163:srx1.c **** 3164:srx1.c **** PrintString(str_parameter_line_threshold); 8078 .LM1385: 8079 293e 80E0 ldi r24,lo8(str_parameter_line_threshold) 8080 2940 90E0 ldi r25,hi8(str_parameter_line_threshold) 8081 2942 0E94 0000 call PrintString 3165:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.line_threshold)); 8083 .LM1386: 8084 2946 80E0 ldi r24,lo8(E_ParameterTable+6) 8085 2948 90E0 ldi r25,hi8(E_ParameterTable+6) 8086 294a 0E94 0000 call eeprom_read_byte 8087 294e 0E94 0000 call PutDecByte 3166:srx1.c **** 3167:srx1.c **** PrintString(str_parameter_scm_exposure_h); 8089 .LM1387: 8090 2952 80E0 ldi r24,lo8(str_parameter_scm_exposure_h) 8091 2954 90E0 ldi r25,hi8(str_parameter_scm_exposure_h) 8092 2956 0E94 0000 call PrintString 3168:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.scm_exposure_h)); 8094 .LM1388: 8095 295a 80E0 ldi r24,lo8(E_ParameterTable+7) 8096 295c 90E0 ldi r25,hi8(E_ParameterTable+7) 8097 295e 0E94 0000 call eeprom_read_byte 8098 2962 0E94 0000 call PutDecByte 3169:srx1.c **** 3170:srx1.c **** PrintString(str_parameter_scm_exposure_l); 8100 .LM1389: 8101 2966 80E0 ldi r24,lo8(str_parameter_scm_exposure_l) 8102 2968 90E0 ldi r25,hi8(str_parameter_scm_exposure_l) 8103 296a 0E94 0000 call PrintString 3171:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.scm_exposure_l)); 8105 .LM1390: 8106 296e 80E0 ldi r24,lo8(E_ParameterTable+8) 8107 2970 90E0 ldi r25,hi8(E_ParameterTable+8) 8108 2972 0E94 0000 call eeprom_read_byte 8109 2976 0E94 0000 call PutDecByte 3172:srx1.c **** 3173:srx1.c **** PrintString(str_parameter_scm_threshold); 8111 .LM1391: 8112 297a 80E0 ldi r24,lo8(str_parameter_scm_threshold) 8113 297c 90E0 ldi r25,hi8(str_parameter_scm_threshold) 8114 297e 0E94 0000 call PrintString 3174:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.scm_threshold)); 8116 .LM1392: 8117 2982 80E0 ldi r24,lo8(E_ParameterTable+9) 8118 2984 90E0 ldi r25,hi8(E_ParameterTable+9) 8119 2986 0E94 0000 call eeprom_read_byte 8120 298a 0E94 0000 call PutDecByte 3175:srx1.c **** 3176:srx1.c **** PrintString(str_parameter_scm_min_size); 8122 .LM1393: 8123 298e 80E0 ldi r24,lo8(str_parameter_scm_min_size) 8124 2990 90E0 ldi r25,hi8(str_parameter_scm_min_size) 8125 2992 0E94 0000 call PrintString 3177:srx1.c **** PutDecByte(eeprom_read_byte(&E_ParameterTable.scm_min_size)); 8127 .LM1394: 8128 2996 80E0 ldi r24,lo8(E_ParameterTable+10) 8129 2998 90E0 ldi r25,hi8(E_ParameterTable+10) 8130 299a 0E94 0000 call eeprom_read_byte 8131 299e 0E94 0000 call PutDecByte 3178:srx1.c **** } 8133 .LM1395: 8134 /* epilogue: frame size=0 */ 8135 29a2 0895 ret 8136 /* epilogue end (size=1) */ 8137 /* function DumpParameterTable size 105 (104) */ 8139 .Lscope22: 8143 .global EditParameterTable 8145 EditParameterTable: 3179:srx1.c **** 3180:srx1.c **** // 3181:srx1.c **** // Function name: EditParameterTable() 3182:srx1.c **** // Description: Edit the specified entry of the EEPROM paramenter table. 3183:srx1.c **** // 3184:srx1.c **** void EditParameterTable(unsigned char id) 3185:srx1.c **** { 8147 .LM1396: 8148 /* prologue: frame size=0 */ 8149 29a4 1F93 push r17 8150 /* prologue end (size=1) */ 8151 29a6 182F mov r17,r24 3186:srx1.c **** PutChar('='); 8153 .LM1397: 8154 29a8 8DE3 ldi r24,lo8(61) 8155 29aa 0E94 0000 call PutChar 3187:srx1.c **** 3188:srx1.c **** switch(id) 8157 .LM1398: 8158 29ae 812F mov r24,r17 8159 29b0 9927 clr r25 8160 29b2 8530 cpi r24,5 8161 29b4 9105 cpc r25,__zero_reg__ 8162 29b6 09F4 brne .+2 8163 29b8 3FC0 rjmp .L512 8164 29ba 8630 cpi r24,6 8165 29bc 9105 cpc r25,__zero_reg__ 8166 29be 94F4 brge .L521 8167 29c0 8230 cpi r24,2 8168 29c2 9105 cpc r25,__zero_reg__ 8169 29c4 39F1 breq .L509 8170 29c6 8330 cpi r24,3 8171 29c8 9105 cpc r25,__zero_reg__ 8172 29ca 34F4 brge .L522 8173 29cc 0097 sbiw r24,0 8174 29ce 09F4 brne .+2 8175 29d0 58C0 rjmp .L506 8176 29d2 0197 sbiw r24,1 8177 29d4 C9F0 breq .L508 8178 29d6 55C0 rjmp .L506 8179 .L522: 8180 29d8 8330 cpi r24,3 8181 29da 9105 cpc r25,__zero_reg__ 8182 29dc 09F1 breq .L510 8183 29de 0497 sbiw r24,4 8184 29e0 29F1 breq .L511 8185 29e2 4FC0 rjmp .L506 8186 .L521: 8187 29e4 8830 cpi r24,8 8188 29e6 9105 cpc r25,__zero_reg__ 8189 29e8 C9F1 breq .L515 8190 29ea 8930 cpi r24,9 8191 29ec 9105 cpc r25,__zero_reg__ 8192 29ee 34F4 brge .L523 8193 29f0 8630 cpi r24,6 8194 29f2 9105 cpc r25,__zero_reg__ 8195 29f4 39F1 breq .L513 8196 29f6 0797 sbiw r24,7 8197 29f8 59F1 breq .L514 8198 29fa 43C0 rjmp .L506 8199 .L523: 8200 29fc 8930 cpi r24,9 8201 29fe 9105 cpc r25,__zero_reg__ 8202 2a00 99F1 breq .L516 8203 2a02 0A97 sbiw r24,10 8204 2a04 B9F1 breq .L517 8205 2a06 3DC0 rjmp .L506 8206 .L508: 3189:srx1.c **** { 3190:srx1.c **** // f_p 3191:srx1.c **** case 1: 3192:srx1.c **** eeprom_write_byte(&E_ParameterTable.f_p, GetDecWord()); 8208 .LM1399: 8209 2a08 0E94 0000 call GetDecWord 8210 2a0c 682F mov r22,r24 8211 2a0e 80E0 ldi r24,lo8(E_ParameterTable+1) 8212 2a10 90E0 ldi r25,hi8(E_ParameterTable+1) 8213 2a12 35C0 rjmp .L524 8214 .L509: 3193:srx1.c **** break; 3194:srx1.c **** 3195:srx1.c **** // f_d 3196:srx1.c **** case 2: 3197:srx1.c **** eeprom_write_byte(&E_ParameterTable.f_d, GetDecWord()); 8216 .LM1400: 8217 2a14 0E94 0000 call GetDecWord 8218 2a18 682F mov r22,r24 8219 2a1a 80E0 ldi r24,lo8(E_ParameterTable+2) 8220 2a1c 90E0 ldi r25,hi8(E_ParameterTable+2) 8221 2a1e 2FC0 rjmp .L524 8222 .L510: 3198:srx1.c **** break; 3199:srx1.c **** 3200:srx1.c **** // Kp 3201:srx1.c **** case 3: 3202:srx1.c **** eeprom_write_byte(&E_ParameterTable.Kp, GetDecWord()); 8224 .LM1401: 8225 2a20 0E94 0000 call GetDecWord 8226 2a24 682F mov r22,r24 8227 2a26 80E0 ldi r24,lo8(E_ParameterTable+3) 8228 2a28 90E0 ldi r25,hi8(E_ParameterTable+3) 8229 2a2a 29C0 rjmp .L524 8230 .L511: 3203:srx1.c **** break; 3204:srx1.c **** 3205:srx1.c **** // Ki 3206:srx1.c **** case 4: 3207:srx1.c **** eeprom_write_byte(&E_ParameterTable.Ki, GetDecWord()); 8232 .LM1402: 8233 2a2c 0E94 0000 call GetDecWord 8234 2a30 682F mov r22,r24 8235 2a32 80E0 ldi r24,lo8(E_ParameterTable+4) 8236 2a34 90E0 ldi r25,hi8(E_ParameterTable+4) 8237 2a36 23C0 rjmp .L524 8238 .L512: 3208:srx1.c **** break; 3209:srx1.c **** 3210:srx1.c **** // Kd 3211:srx1.c **** case 5: 3212:srx1.c **** eeprom_write_byte(&E_ParameterTable.Kd, GetDecWord()); 8240 .LM1403: 8241 2a38 0E94 0000 call GetDecWord 8242 2a3c 682F mov r22,r24 8243 2a3e 80E0 ldi r24,lo8(E_ParameterTable+5) 8244 2a40 90E0 ldi r25,hi8(E_ParameterTable+5) 8245 2a42 1DC0 rjmp .L524 8246 .L513: 3213:srx1.c **** break; 3214:srx1.c **** 3215:srx1.c **** // Line Sensor Threshold 3216:srx1.c **** case 6: 3217:srx1.c **** eeprom_write_byte(&E_ParameterTable.line_threshold, GetDecWord()); 8248 .LM1404: 8249 2a44 0E94 0000 call GetDecWord 8250 2a48 682F mov r22,r24 8251 2a4a 80E0 ldi r24,lo8(E_ParameterTable+6) 8252 2a4c 90E0 ldi r25,hi8(E_ParameterTable+6) 8253 2a4e 17C0 rjmp .L524 8254 .L514: 3218:srx1.c **** break; 3219:srx1.c **** 3220:srx1.c **** // SCM Initial exposure High 3221:srx1.c **** case 7: 3222:srx1.c **** eeprom_write_byte(&E_ParameterTable.scm_exposure_h, GetDecWord()); 8256 .LM1405: 8257 2a50 0E94 0000 call GetDecWord 8258 2a54 682F mov r22,r24 8259 2a56 80E0 ldi r24,lo8(E_ParameterTable+7) 8260 2a58 90E0 ldi r25,hi8(E_ParameterTable+7) 8261 2a5a 11C0 rjmp .L524 8262 .L515: 3223:srx1.c **** break; 3224:srx1.c **** 3225:srx1.c **** // SCM Initial exposure Low 3226:srx1.c **** case 8: 3227:srx1.c **** eeprom_write_byte(&E_ParameterTable.scm_exposure_l, GetDecWord()); 8264 .LM1406: 8265 2a5c 0E94 0000 call GetDecWord 8266 2a60 682F mov r22,r24 8267 2a62 80E0 ldi r24,lo8(E_ParameterTable+8) 8268 2a64 90E0 ldi r25,hi8(E_ParameterTable+8) 8269 2a66 0BC0 rjmp .L524 8270 .L516: 3228:srx1.c **** break; 3229:srx1.c **** 3230:srx1.c **** // SCM Brightness Threshold 3231:srx1.c **** case 9: 3232:srx1.c **** eeprom_write_byte(&E_ParameterTable.scm_threshold, GetDecWord()); 8272 .LM1407: 8273 2a68 0E94 0000 call GetDecWord 8274 2a6c 682F mov r22,r24 8275 2a6e 80E0 ldi r24,lo8(E_ParameterTable+9) 8276 2a70 90E0 ldi r25,hi8(E_ParameterTable+9) 8277 2a72 05C0 rjmp .L524 8278 .L517: 3233:srx1.c **** break; 3234:srx1.c **** 3235:srx1.c **** // SCM Size Threshold 3236:srx1.c **** case 10: 3237:srx1.c **** eeprom_write_byte(&E_ParameterTable.scm_min_size, GetDecWord()); 8280 .LM1408: 8281 2a74 0E94 0000 call GetDecWord 8282 2a78 682F mov r22,r24 8283 2a7a 80E0 ldi r24,lo8(E_ParameterTable+10) 8284 2a7c 90E0 ldi r25,hi8(E_ParameterTable+10) 8285 .L524: 8286 2a7e 0E94 0000 call eeprom_write_byte 3238:srx1.c **** break; 3239:srx1.c **** 3240:srx1.c **** case 0: 3241:srx1.c **** default: 3242:srx1.c **** break; 3243:srx1.c **** } 3244:srx1.c **** } 8288 .LM1409: 8289 .L506: 8290 /* epilogue: frame size=0 */ 8291 2a82 1F91 pop r17 8292 2a84 0895 ret 8293 /* epilogue end (size=2) */ 8294 /* function EditParameterTable size 117 (114) */ 8296 .Lscope23: 8298 .comm R_ParameterTable,11,1 8299 .global E_ParameterTable 8300 .section .eeprom,"aw",@progbits 8303 E_ParameterTable: 8304 0000 0000 0000 .skip 11,0 8304 0000 0000 8304 0000 00 8305 .comm execStk,85,1 8306 .comm execPid,6,1 8307 .comm arbitrateStk,75,1 8308 .comm arbitratePid,6,1 8309 .comm followStk,65,1 8310 .comm followPid,6,1 8311 .comm goatStk,75,1 8312 .comm goatPid,6,1 8313 .comm forwardStk,65,1 8314 .comm forwardPid,6,1 8315 .comm rotateStk,65,1 8316 .comm rotatePid,6,1 8317 .comm avoidStk,75,1 8318 .comm avoidPid,6,1 8319 .comm searchStk,75,1 8320 .comm searchPid,6,1 8321 .comm extinguishStk,65,1 8322 .comm extinguishPid,6,1 8323 .comm plannerStk,65,1 8324 .comm plannerPid,6,1 8325 .comm alignStk,65,1 8326 .comm alignPid,6,1 8327 .comm exec_timer,6,1 8328 .comm goat_timer,6,1 8329 .comm follow_timer,6,1 8330 .comm avoid_timer,6,1 8331 .comm extinguish_timer,6,1 8332 .comm planner_timer,6,1 8333 .comm align_timer,6,1 8334 .comm Room,1,1 8335 .comm last_line_trigger,1,1 8336 .comm BehaviourTable,90,1 8337 .comm RotateQueue,4,1 8338 .comm ForwardQueue,4,1 8339 .comm FollowQueue,4,1 8340 .comm ArbitrateQueue,4,1 8341 .comm GoAtQueue,4,1 8342 .comm ExtinguishQueue,4,1 8343 .comm AvoidQueue,4,1 8344 .comm SearchQueue,4,1 8345 .comm AlignQueue,4,1 8456 .text 8458 Letext: 8459 /* File "srx1.c": code 5539 = 0x15a3 (5511), prologues 10, epilogues 18 */ DEFINED SYMBOLS *ABS*:00000000 srx1.c *ABS*:0000003f __SREG__ *ABS*:0000003e __SP_H__ *ABS*:0000003d __SP_L__ *ABS*:00000000 __tmp_reg__ *ABS*:00000001 __zero_reg__ C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:137 .progmem.data:00000000 str_exec_login C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:141 .progmem.data:0000001d str_exec_login_bar C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:145 .progmem.data:00000047 str_exec_quit C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:149 .progmem.data:00000057 str_soft_reset C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:153 .progmem.data:00000066 str_enter_port_letter C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:157 .progmem.data:0000007c str_port_read_banner C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:161 .progmem.data:00000083 str_port_read_banner2 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:165 .progmem.data:00000091 str_spacer C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:169 .progmem.data:000000ba str_behaviour_status2 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:174 .progmem.data:0000011b str_enter_address C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:178 .progmem.data:00000131 str_enter_element C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:182 .progmem.data:00000143 str_enter_byte C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:186 .progmem.data:00000152 str_line_sensors C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:190 .progmem.data:00000163 str_error C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:194 .progmem.data:0000016d str_cartesian_position C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:198 .progmem.data:00000184 str_gpd_readings C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:202 .progmem.data:00000197 str_parameter_header C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:206 .progmem.data:000001c6 str_parameter_f_p C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:210 .progmem.data:000001e3 str_parameter_f_d C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:214 .progmem.data:00000200 str_parameter_Kp C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:218 .progmem.data:0000021c str_parameter_Ki C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:222 .progmem.data:00000238 str_parameter_Kd C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:226 .progmem.data:00000254 str_parameter_line_threshold C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:230 .progmem.data:00000272 str_parameter_scm_exposure_h C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:234 .progmem.data:0000028c str_parameter_scm_exposure_l C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:238 .progmem.data:000002a5 str_parameter_scm_threshold C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:242 .progmem.data:000002bc str_parameter_scm_min_size C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:246 .progmem.data:000002de str_parameter_edit C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:250 .progmem.data:00000308 str_help1 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:254 .progmem.data:00000326 str_help2 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:258 .progmem.data:0000034b str_help3 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:262 .progmem.data:0000037a str_help4 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:266 .progmem.data:0000039f str_help5 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:270 .progmem.data:000003c4 str_help6 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:274 .progmem.data:000003ec str_help7 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:278 .progmem.data:00000415 str_help8 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:282 .progmem.data:00000445 str_help9 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:286 .progmem.data:0000046f str_help10 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:290 .progmem.data:000004a2 str_help11 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:294 .progmem.data:000004cb str_help12 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:298 .progmem.data:00000502 str_help13 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:302 .progmem.data:0000053c str_help14 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:306 .progmem.data:00000571 str_help15 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:310 .progmem.data:000005a3 str_exec C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:314 .progmem.data:000005a8 str_avoid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:318 .progmem.data:000005ae str_forward C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:322 .progmem.data:000005b6 str_goat C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:326 .progmem.data:000005bc str_extinguish C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:330 .progmem.data:000005c7 str_align C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:334 .progmem.data:000005cd str_roam C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:338 .progmem.data:000005d2 str_rotate C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:342 .progmem.data:000005d9 str_search C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:346 .progmem.data:000005e0 str_follow C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:358 .progmem.data:000005e7 execTcb *COM*:00000055 execStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:521 .text:0000001e exec *COM*:00000006 execPid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:366 .progmem.data:000005ee arbitrateTcb *COM*:0000004b arbitrateStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:6623 .text:0000222a arbitrate *COM*:00000006 arbitratePid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:374 .progmem.data:000005f5 followTcb *COM*:00000041 followStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:4629 .text:000016d8 follow *COM*:00000006 followPid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:382 .progmem.data:000005fc goatTcb *COM*:0000004b goatStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:5517 .text:00001bb6 goat *COM*:00000006 goatPid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:390 .progmem.data:00000603 forwardTcb *COM*:00000041 forwardStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:3927 .text:00001336 forward *COM*:00000006 forwardPid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:398 .progmem.data:0000060a rotateTcb *COM*:00000041 rotateStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:4342 .text:0000154c rotate *COM*:00000006 rotatePid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:406 .progmem.data:00000611 avoidTcb *COM*:0000004b avoidStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:4198 .text:000014a8 avoid *COM*:00000006 avoidPid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:414 .progmem.data:00000618 searchTcb *COM*:0000004b searchStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:6483 .text:00002182 search *COM*:00000006 searchPid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:422 .progmem.data:0000061f extinguishTcb *COM*:00000041 extinguishStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:5925 .text:00001e3c extinguish *COM*:00000006 extinguishPid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:430 .progmem.data:00000626 plannerTcb *COM*:00000041 plannerStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:3562 .text:0000111e planner *COM*:00000006 plannerPid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:438 .progmem.data:0000062d alignTcb *COM*:00000041 alignStk C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:5017 .text:000018e2 align *COM*:00000006 alignPid C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:448 .bss:00000000 velocity_print C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:454 .bss:00000001 arbitrate_debug C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:460 .bss:00000002 follow_debug C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:466 .text:00000000 __vector_16 .bss:00000003 RotateCmd.0 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:510 .bss:0000000c ForwardCmd.1 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:511 .bss:00000014 FollowCmd.2 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:512 .bss:0000001b ArbitrateCmd.3 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:513 .bss:00000029 GoAtCmd.4 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:514 .bss:00000033 ExtinguishCmd.5 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:515 .bss:00000038 SearchCmd.6 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:516 .bss:0000003e AlignCmd.7 *COM*:00000004 ArbitrateQueue *COM*:0000005a BehaviourTable *COM*:0000000b R_ParameterTable *COM*:00000004 RotateQueue *COM*:00000004 ForwardQueue *COM*:00000004 FollowQueue *COM*:00000004 AlignQueue *COM*:00000004 GoAtQueue *COM*:00000004 SearchQueue *COM*:00000004 ExtinguishQueue C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7935 .text:0000286c InitParameterTable C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7909 .text:0000285a LoadParameterTable C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:8012 .text:000028d2 DumpParameterTable C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:8145 .text:000029a4 EditParameterTable *COM*:00000001 Room C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7630 .text:000026d4 GoHome *COM*:00000006 planner_timer C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7318 .text:0000259a Forward C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7534 .text:00002672 Follow C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7412 .text:00002602 Rotate C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7243 .text:0000254e Align C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7463 .text:00002632 Search C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:517 .bss:00000044 ArbitrateCmd.8 *COM*:00000001 last_line_trigger C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:3923 .bss:00000052 ArbitrateCmd.9 *COM*:00000006 avoid_timer C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:4194 .bss:00000060 ArbitrateCmd.10 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:4338 .bss:0000006e ArbitrateCmd.11 *COM*:00000006 follow_timer C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:4625 .bss:0000007c ArbitrateCmd.12 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:5012 .bss:0000008a ForwardCmd.13 *COM*:00000006 align_timer C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:5013 .bss:00000092 ArbitrateCmd.14 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:5512 .bss:000000a0 RotateCmd.15 *COM*:00000006 goat_timer C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:5513 .bss:000000a9 ArbitrateCmd.16 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:5920 .bss:000000b7 RotateCmd.17 *COM*:00000006 extinguish_timer C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:5921 .bss:000000c0 ForwardCmd.18 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:6477 .bss:000000c8 RotateCmd.19 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:6478 .bss:000000d1 AlignCmd.20 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:6479 .bss:000000d7 pArbitrateCmd.21 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:6618 .bss:000000d9 ArbitrateCmd.22 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:6943 .text:00002404 InitBehaviourTable C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7060 .text:0000247c main C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:6619 .bss:000000e7 AlignCmd.23 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7238 .bss:000000ed ForwardCmd.24 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7312 .bss:000000f5 RotateCmd.25 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7406 .bss:000000fe SearchCmd.26 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7457 .bss:00000104 ExtinguishCmd.27 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7458 .bss:00000109 FollowCmd.28 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7528 .bss:00000110 GoAtCmd.29 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:7624 .progmem.data:00000634 __c.30 C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:8303 .eeprom:00000000 E_ParameterTable *COM*:00000006 exec_timer *COM*:00000004 AvoidQueue C:\DOCUME~1\palm3\LOCALS~1\Temp/ccWabaaa.s:8458 .text:00002a86 Letext *ABS*:00000000 *ABS* UNDEFINED SYMBOLS __do_copy_data __do_clear_bss IntProlog AvrXTimerHandler QuadratureDecoder Epilog GetChar AvrXSendMessage AvrXWaitMessageAck PrintString PutCRLF PutChar _GetChar iX PutDecSignedWord iY iTheta PutDecByte __udivmodqi4 PutDecSignedByte adc_read gpd_read i2c_readbyte PutHexByte PutBin GetHexByte i2c_writebyte i2c_readword PutHexWord GetHexWord i2c_writeword GetDecSignedWord GetDecWord PutDecWord AvrXSuspend AvrXRunTask SCM_calibrate ResetOdometry Line_calibrate AvrXDelay AvrXWaitMessage __floatsisf __divsf3 __fixsfsi SynchWithMotion AvrXAckMessage WaitForMotionToStop __divmodhi4 EmergencyStop atan2 __mulsf3 motors Left Right __stack AvrXSetKernelStack InitADC InitSerialIO i2c_init SCM_Init MotorTaskTcb exit AvrXHalt eeprom_read_block eeprom_write_byte eeprom_read_byte