├── Surrogates ├── Firebird-Code │ ├── Firebird_Rpi │ │ ├── Debug │ │ │ ├── Firebird_Rpi.eep │ │ │ ├── Firebird_Rpi.o │ │ │ ├── Firebird_Rpi.elf │ │ │ ├── makedep.mk │ │ │ ├── Makefile │ │ │ ├── Firebird_Rpi.d │ │ │ ├── Firebird_Rpi.hex │ │ │ ├── Firebird_Rpi.map │ │ │ └── Firebird_Rpi.lss │ │ ├── Firebird_Rpi.atsuo │ │ ├── Firebird_Rpi.atsln │ │ ├── Firebird_Rpi.cproj │ │ └── Firebird_Rpi.c │ ├── forthewin.atsuo │ └── Firebird_Rpi.atsuo ├── Python-Code │ └── Python-Code-version1.0 │ │ ├── fipi.pyc │ │ ├── servo_blasteroid.py │ │ ├── Sample_Experiments.py │ │ ├── line-follower.py │ │ └── fipi.py └── Read Me.txt ├── .gitattributes └── README.md /Surrogates/Firebird-Code/Firebird_Rpi/Debug/Firebird_Rpi.eep: -------------------------------------------------------------------------------- 1 | :00000001FF 2 | -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/forthewin.atsuo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epiception/Virtual-Telepresence/HEAD/Surrogates/Firebird-Code/forthewin.atsuo -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi.atsuo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epiception/Virtual-Telepresence/HEAD/Surrogates/Firebird-Code/Firebird_Rpi.atsuo -------------------------------------------------------------------------------- /Surrogates/Python-Code/Python-Code-version1.0/fipi.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epiception/Virtual-Telepresence/HEAD/Surrogates/Python-Code/Python-Code-version1.0/fipi.pyc -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Debug/Firebird_Rpi.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epiception/Virtual-Telepresence/HEAD/Surrogates/Firebird-Code/Firebird_Rpi/Debug/Firebird_Rpi.o -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Firebird_Rpi.atsuo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epiception/Virtual-Telepresence/HEAD/Surrogates/Firebird-Code/Firebird_Rpi/Firebird_Rpi.atsuo -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Debug/Firebird_Rpi.elf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epiception/Virtual-Telepresence/HEAD/Surrogates/Firebird-Code/Firebird_Rpi/Debug/Firebird_Rpi.elf -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Debug/makedep.mk: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Automatically-generated file. Do not edit or delete the file 3 | ################################################################################ 4 | 5 | Firebird_Rpi.c 6 | 7 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Firebird_Rpi.atsln: -------------------------------------------------------------------------------- 1 | 2 | Microsoft Visual Studio Solution File, Format Version 11.00 3 | # Atmel Studio Solution File, Format Version 11.00 4 | Project("{54F91283-7BC4-4236-8FF9-10F437C3AD48}") = "Firebird_Rpi", "Firebird_Rpi.cproj", "{72EEE922-D444-4FFC-B6B0-21F5CCA34DAD}" 5 | EndProject 6 | Global 7 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 8 | Debug|AVR = Debug|AVR 9 | Release|AVR = Release|AVR 10 | EndGlobalSection 11 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 12 | {72EEE922-D444-4FFC-B6B0-21F5CCA34DAD}.Debug|AVR.ActiveCfg = Debug|AVR 13 | {72EEE922-D444-4FFC-B6B0-21F5CCA34DAD}.Debug|AVR.Build.0 = Debug|AVR 14 | {72EEE922-D444-4FFC-B6B0-21F5CCA34DAD}.Release|AVR.ActiveCfg = Release|AVR 15 | {72EEE922-D444-4FFC-B6B0-21F5CCA34DAD}.Release|AVR.Build.0 = Release|AVR 16 | EndGlobalSection 17 | GlobalSection(SolutionProperties) = preSolution 18 | HideSolutionNode = FALSE 19 | EndGlobalSection 20 | EndGlobal 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Virtual-Telepresence 2 | ## 3d Telepresence(SBS) Platform using Google Cardboard and Raspberry Pi 2 3 | 4 | ### View our current Implementation: 5 | IMAGE ALT TEXT HERE 8 | 9 | ### Requirements: 10 | 11 | #### Hardware: 12 | 1. Firebird V Platform ([Nex Robotics](http://www.nex-robotics.com/fire-bird-v-atmega2560/fire-bird-v-atmega2560.html)) 13 | 2. Raspberry Pi 2 14 | 3. Raspicam camera module 15 | 4. USB-UART cable 16 | 5. Jumper Wires 17 | 6. 2 servo motors (pan and tilt) 18 | 7. camera gimbal platform(self made) 19 | 8. Wifi dongle 20 | 21 | #### Software: 22 | 23 | 1. Scripts: Python([Startup Script](http://www.instructables.com/id/Raspberry-Pi-Launch-Python-script-on-startup/?ALLSTEPS)) 24 | * servo_blasteroid: For recieving udp packets and running servos on gpio pin 4 and 17 25 | * Sample_Experiments.py: modified using pygame library for control using keyboard (W,A,S,D). Modified based on prior work by Saurav Shandilya. 26 | 27 | 2. Libraries/Utilities/Dependancies: 28 | * [ServoBlaster](https://github.com/richardghirst/PiBits/tree/master/ServoBlaster) 29 | * [FireBird-RPi Interface](https://github.com/sauravshandilya/Fi-Pi) (modified) 30 | * [Pygame](http://www.pygame.org/wiki/about) 31 | * [UV4L](http://www.linux-projects.org/modules/sections/index.php?op=viewarticle&artid=14) 32 | 33 | 3. Apps: (Available for Android Market) 34 | * Wireless IMU: https://play.google.com/store/apps/details?id=org.zwiener.wimu&hl=en 35 | * DualScreen: https://play.google.com/store/apps/details?id=com.goestoweb.dualbrowser&hl=en 36 | -------------------------------------------------------------------------------- /Surrogates/Read Me.txt: -------------------------------------------------------------------------------- 1 | 2 | Raspberry Pi and ATMega powered Stereoscopic Telepresence (2 axis) 3 | 3d Telepresence(SBS) using Google Cardboard and Raspberry Pi 2 4 | 5 | (I really need expertise in perfecting this project from you fellow tinkers. Any contributions are gladly accepted with a transfer of ASCII cookies. Here, have a cookie :') 6 | 7 | -=[ cookies & milk ]=- 8 | 9 | .-'''''-. 10 | |'-----'| 11 | |-.....-| 12 | | | 13 | | | 14 | _,._ | | 15 | __.o` o`"-. | | 16 | .-O o `"-.o O )_,._ | | 17 | ( o O o )--.-"`O o"-.`'-----'` 18 | '--------' ( o O o) 19 | `----------` 20 | 21 | 22 | Requirements: 23 | Hardware: 24 | Firebird V Platform (Nex Robotics) 25 | http://www.nex-robotics.com/fire-bird-v-atmega2560/fire-bird-v-atmega2560.html 26 | Raspberry Pi 2 27 | Raspicam camera module 28 | USB-UART cable 29 | Jumper Wires 30 | 2 servo motors 31 | camera gimbal platform(self made) 32 | Wifi dongle 33 | 34 | Software: 35 | 36 | Scripts: Pyhton(Bash for automation of scripts on Raspi boot: http://www.instructables.com/id/Raspberry-Pi-Launch-Python-script-on-startup/?ALLSTEPS) 37 | 38 | 1. servo_blasteroid: For recieving udp packets and running servos on gpio pin 4 and 17 39 | 2. Sample_Experiments.py: modified using pygame library for control using keyboard (W,A,S,D). Modified based on prior work by Saurav Shandilya. 40 | 41 | 42 | Libraries/Utilities/Dependancies: 43 | ServoBlaster: https://github.com/richardghirst/PiBits/tree/master/ServoBlaster 44 | FireBird-RPi Interface: https://github.com/sauravshandilya/Fi-Pi (modified) 45 | Pygame: http://www.pygame.org/wiki/about 46 | UV4L: http://www.linux-projects.org/modules/sections/index.php?op=viewarticle&artid=14 47 | 48 | Apps: (Available for Android Market) 49 | 50 | Wireless IMU: https://play.google.com/store/apps/details?id=org.zwiener.wimu&hl=en 51 | DualScreen: https://play.google.com/store/apps/details?id=com.goestoweb.dualbrowser&hl=en 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /Surrogates/Python-Code/Python-Code-version1.0/servo_blasteroid.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Application: Development of Raspberry Pi controlled Firebird-V Robot. 3 | Hardware: Firebird-V Atmega2560 Robot and Raspberry Pi 2 4 | Python Version: 2.7 5 | Requirements: 6 | App: Wireless IMU on the Android enabled smartphone 7 | PI: Servoblaster package for Hardware PWM pulses ON GPIO (source:github/richardghirst/PiBits) 8 | 9 | Description: For Gimbal Control using Cardboard and Android Smartphone 10 | This file accepts udp PACKETS from the phone and uses those values to map to angles and control 2 servos(Pan and Tilt) 11 | on gpio pin 4(pwm hardwaee channel 0) and gpio pin 17 12 | 13 | (name inspired by Mastodon) 14 | -------------------------------------------------------------------- 15 | Usage: 16 | Run the program 17 | 18 | ''' 19 | import socket,traceback,os 20 | from time import sleep 21 | import datetime 22 | import math 23 | #start servo_blaster 24 | os.system('sudo ./servod') 25 | os.system('echo 1=150 > /dev/servoblaster') 26 | os.system('echo 0=150 > /dev/servoblaster') 27 | 28 | sleep(1) 29 | 30 | host='' 31 | port = 5555 32 | 33 | s=socket.socket(socket.AF_INET,socket.SOCK_DGRAM) 34 | s.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR,1) 35 | s.setsockopt(socket.SOL_SOCKET,socket.SO_BROADCAST,1) 36 | s.bind((host,port)) 37 | 38 | def value_map(x, in_min, in_max, out_min, out_max): 39 | val=int((x-in_min)*(out_max-out_min)/(in_max-in_min)+out_min); 40 | if val>250: 41 | val=250 42 | elif val<50: 43 | val=50 44 | return val 45 | 46 | sleep(0.5) 47 | LP=0.20 48 | gyrolimit=0.0 49 | gyroXangle=0.0 50 | gyrolimit=0.0002 51 | while 1: 52 | a=datetime.datetime.now() 53 | try: 54 | message,address=s.recvfrom(8192) 55 | axl_x=float(message[17:23]) 56 | axl_y=float(message[25:31]) 57 | axl_z=float(message[33:39]) 58 | try: 59 | gyr_x=float(message[45:51]) 60 | #gyr_y=float(message[53:59]) 61 | #gyr_z=float(message[61:67]) 62 | except: 63 | pass 64 | gyroXangle+=gyr_x*LP 65 | degrees=value_map(axl_z,-9.5,+9.5,50,250) 66 | degrees2=value_map(gyroXangle,gyrolimit-17.0,gyrolimit+18.0,250,50) 67 | #pitch=180*math.atan(axl_x/math.sqrt(axl_y*axl_y+axl_z*axl_z)) uncomment for complimentary filter calculations 68 | #roll=180*math.atan(axl_y/math.sqrt(axl_x*axl_x+axl_z*axl_z)) uncomment for complimentary filter calculations 69 | print "degrees: %f degrees2: %f "%(degrees,degrees2) 70 | string = "echo 0=%d > /dev/servoblaster" % (degrees2) 71 | os.system(string) 72 | 73 | string = "echo 1=%d > /dev/servoblaster" % (degrees) 74 | os.system(string) 75 | 76 | 77 | except(KeyboardInterrupt,SystemExit): 78 | os.system('sudo killall servod') 79 | break 80 | raise 81 | except: 82 | traceback.print_exc() 83 | b=datetime.datetime.now() 84 | print((b-a).microseconds/1000) 85 | time.sleep(1) 86 | os.system('sudo killall servod') 87 | 88 | -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Debug/Makefile: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Automatically-generated file. Do not edit! 3 | ################################################################################ 4 | 5 | SHELL := cmd.exe 6 | RM := rm -rf 7 | 8 | USER_OBJS := 9 | 10 | LIBS := 11 | PROJ := 12 | 13 | O_SRCS := 14 | C_SRCS := 15 | S_SRCS := 16 | S_UPPER_SRCS := 17 | OBJ_SRCS := 18 | ASM_SRCS := 19 | PREPROCESSING_SRCS := 20 | OBJS := 21 | OBJS_AS_ARGS := 22 | C_DEPS := 23 | C_DEPS_AS_ARGS := 24 | EXECUTABLES := 25 | OUTPUT_FILE_PATH := 26 | OUTPUT_FILE_PATH_AS_ARGS := 27 | AVR_APP_PATH :=$$$AVR_APP_PATH$$$ 28 | QUOTE := " 29 | ADDITIONAL_DEPENDENCIES:= 30 | OUTPUT_FILE_DEP:= 31 | 32 | # Every subdirectory with source files must be described here 33 | SUBDIRS := 34 | 35 | 36 | # Add inputs and outputs from these tool invocations to the build variables 37 | C_SRCS += \ 38 | ../Firebird_Rpi.c 39 | 40 | 41 | PREPROCESSING_SRCS += 42 | 43 | 44 | ASM_SRCS += 45 | 46 | 47 | OBJS += \ 48 | Firebird_Rpi.o 49 | 50 | 51 | OBJS_AS_ARGS += \ 52 | Firebird_Rpi.o 53 | 54 | 55 | C_DEPS += \ 56 | Firebird_Rpi.d 57 | 58 | 59 | C_DEPS_AS_ARGS += \ 60 | Firebird_Rpi.d 61 | 62 | 63 | OUTPUT_FILE_PATH +=Firebird_Rpi.elf 64 | 65 | OUTPUT_FILE_PATH_AS_ARGS +=Firebird_Rpi.elf 66 | 67 | ADDITIONAL_DEPENDENCIES:= 68 | 69 | OUTPUT_FILE_DEP:= ./makedep.mk 70 | 71 | # AVR32/GNU C Compiler 72 | 73 | 74 | 75 | ./%.o: .././%.c 76 | @echo Building file: $< 77 | @echo Invoking: AVR/GNU C Compiler : (AVR_8_bit_GNU_Toolchain_3.4.0_663) 4.6.2 78 | $(QUOTE)C:\Program Files\Atmel\Atmel Studio 6.0\extensions\Atmel\AVRGCC\3.4.0.65\AVRToolchain\bin\avr-gcc.exe$(QUOTE) -funsigned-char -funsigned-bitfields -O1 -fpack-struct -fshort-enums -g2 -Wall -c -std=gnu99 -MD -MP -MF "$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -MT"$(@:%.o=%.o)" -mmcu=atmega2560 -o"$@" "$<" 79 | @echo Finished building: $< 80 | 81 | 82 | 83 | # AVR32/GNU Preprocessing Assembler 84 | 85 | 86 | 87 | # AVR32/GNU Assembler 88 | 89 | 90 | 91 | 92 | ifneq ($(MAKECMDGOALS),clean) 93 | ifneq ($(strip $(C_DEPS)),) 94 | -include $(C_DEPS) 95 | endif 96 | endif 97 | 98 | # Add inputs and outputs from these tool invocations to the build variables 99 | 100 | # All Target 101 | all: $(OUTPUT_FILE_PATH) $(ADDITIONAL_DEPENDENCIES) 102 | 103 | $(OUTPUT_FILE_PATH): $(OBJS) $(USER_OBJS) $(OUTPUT_FILE_DEP) 104 | @echo Building target: $@ 105 | @echo Invoking: AVR/GNU Linker : (AVR_8_bit_GNU_Toolchain_3.4.0_663) 4.6.2 106 | $(QUOTE)C:\Program Files\Atmel\Atmel Studio 6.0\extensions\Atmel\AVRGCC\3.4.0.65\AVRToolchain\bin\avr-gcc.exe$(QUOTE) -o$(OUTPUT_FILE_PATH_AS_ARGS) $(OBJS_AS_ARGS) $(USER_OBJS) $(LIBS) -Wl,-Map="Firebird_Rpi.map" -Wl,--start-group -Wl,-lm -Wl,--end-group -mmcu=atmega2560 107 | @echo Finished building target: $@ 108 | "C:\Program Files\Atmel\Atmel Studio 6.0\extensions\Atmel\AVRGCC\3.4.0.65\AVRToolchain\bin\avr-objcopy.exe" -O ihex -R .eeprom -R .fuse -R .lock -R .signature "Firebird_Rpi.elf" "Firebird_Rpi.hex" 109 | "C:\Program Files\Atmel\Atmel Studio 6.0\extensions\Atmel\AVRGCC\3.4.0.65\AVRToolchain\bin\avr-objcopy.exe" -j .eeprom --set-section-flags=.eeprom=alloc,load --change-section-lma .eeprom=0 --no-change-warnings -O ihex "Firebird_Rpi.elf" "Firebird_Rpi.eep" || exit 0 110 | "C:\Program Files\Atmel\Atmel Studio 6.0\extensions\Atmel\AVRGCC\3.4.0.65\AVRToolchain\bin\avr-objdump.exe" -h -S "Firebird_Rpi.elf" > "Firebird_Rpi.lss" 111 | "C:\Program Files\Atmel\Atmel Studio 6.0\extensions\Atmel\AVRGCC\3.4.0.65\AVRToolchain\bin\avr-size.exe" "Firebird_Rpi.elf" 112 | 113 | 114 | 115 | 116 | 117 | 118 | # Other Targets 119 | clean: 120 | -$(RM) $(OBJS_AS_ARGS)$(C_DEPS_AS_ARGS) $(EXECUTABLES) 121 | rm -rf "Firebird_Rpi.elf" "Firebird_Rpi.a" "Firebird_Rpi.hex" "Firebird_Rpi.lss" "Firebird_Rpi.eep" "Firebird_Rpi.map" 122 | -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Firebird_Rpi.cproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 2.0 5 | 6.0 6 | com.Atmel.AVRGCC8 7 | {72eee922-d444-4ffc-b6b0-21f5cca34dad} 8 | ATmega2560 9 | none 10 | Executable 11 | C 12 | $(MSBuildProjectName) 13 | .elf 14 | $(MSBuildProjectDirectory)\$(Configuration) 15 | forthewin 16 | forthewin 17 | forthewin 18 | Native 19 | true 20 | false 21 | 22 | 0 23 | 3.1.3 24 | 25 | 26 | 27 | 28 | True 29 | True 30 | True 31 | True 32 | True 33 | Optimize for size (-Os) 34 | True 35 | True 36 | True 37 | 38 | 39 | m 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | True 49 | True 50 | True 51 | True 52 | True 53 | Optimize (-O1) 54 | True 55 | True 56 | Default (-g2) 57 | True 58 | 59 | 60 | m 61 | 62 | 63 | Default (-Wa,-g) 64 | 65 | 66 | 67 | 68 | 69 | compile 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Debug/Firebird_Rpi.d: -------------------------------------------------------------------------------- 1 | forthewin.d forthewin.o: .././forthewin.c \ 2 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/io.h \ 3 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/sfr_defs.h \ 4 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/inttypes.h \ 5 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/include/stdint.h \ 6 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/stdint.h \ 7 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/iom2560.h \ 8 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/iomxx0_1.h \ 9 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/portpins.h \ 10 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/common.h \ 11 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/version.h \ 12 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/fuse.h \ 13 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/lock.h \ 14 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/interrupt.h \ 15 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/util/delay.h \ 16 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/util/delay_basic.h \ 17 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/math.h 18 | 19 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/io.h: 20 | 21 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/sfr_defs.h: 22 | 23 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/inttypes.h: 24 | 25 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/include/stdint.h: 26 | 27 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/stdint.h: 28 | 29 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/iom2560.h: 30 | 31 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/iomxx0_1.h: 32 | 33 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/portpins.h: 34 | 35 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/common.h: 36 | 37 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/version.h: 38 | 39 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/fuse.h: 40 | 41 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/lock.h: 42 | 43 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/avr/interrupt.h: 44 | 45 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/util/delay.h: 46 | 47 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/util/delay_basic.h: 48 | 49 | c:\program\ files\atmel\atmel\ studio\ 6.0\extensions\atmel\avrgcc\3.4.0.65\avrtoolchain\bin\../lib/gcc/avr/4.6.2/../../../../avr/include/math.h: 50 | -------------------------------------------------------------------------------- /Surrogates/Python-Code/Python-Code-version1.0/Sample_Experiments.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Application: Development of Raspberry Pi controlled Firebird-V Robot. 3 | Hardware: Firebird-V Atmega2560 Robot and Raspberry Pi 1 4 | Python Version: 2.7 5 | Use this file to control robot using keyboard 6 | 7 | Description: 8 | 1. This is a experiment file which will test all hardware and their components. 9 | 2. Current Implementation includes hardware test like Buzzer, DC motors with velocity control, Servo motors, Interrupt driven position encoder, BarLED, Sensors reading from both ATmega2560(master) and ATmega 8(slave) microcontroller. 10 | 11 | -------------------------------------------------------------------- 12 | Usage: 13 | Run the program 14 | Prompt will ask to enter a number: 15 | 16 | 0 - Stop DC motor, turn off buzzer and barLED, 17 | 1 - DC motor direction control and Velocity control using pygame 18 | 2 - Travel bot in forward/backward direction by given distance (note distance is in millimeters) 19 | 3 - Servo motor control 20 | 4 - BarLED control 21 | 5 - ADC sensors value 22 | 23 | ''' 24 | 25 | import fipi as pi 26 | import time 27 | #import os,sys 28 | #os.environ["SDL_VIDEODRIVER"] = "dummy" 29 | import sys,pygame 30 | pygame.init() 31 | from pygame.locals import * 32 | import pygame.transform 33 | size = width, height = 1,1 34 | pygame.display.init() 35 | screen = pygame.display.set_mode(size) 36 | 37 | pi.serial_open() # Open serial port 38 | x=0 39 | screen=pygame.display.set_mode(size) 40 | while True: 41 | 42 | y= raw_input("Enter the number:") 43 | 44 | # 0 - Stop Motion and buzzer 45 | if y == '0': 46 | pi.stop() 47 | pi.buzzer_off 48 | pi.led_bargraph_off(pi.barLED1|pi.barLED2|pi.barLED3|pi.barLED4|pi.barLED5|pi.barLED6|pi.barLED7|pi.barLED8) 49 | print "good bye" 50 | break 51 | 52 | # 1 - Motion Control Test 53 | if y == '1': 54 | while 1: 55 | screen=pygame.display.set_mode(size) 56 | for event in pygame.event.get(): 57 | pass 58 | if event.type == pygame.QUIT: sys.exit() 59 | if event.type == QUIT: 60 | done = True 61 | elif event.type == KEYDOWN: 62 | if event.key == K_ESCAPE: 63 | done = True 64 | 65 | # get key current state 66 | keys = pygame.key.get_pressed() 67 | 68 | if keys[K_LEFT]: 69 | x=1 70 | #print 'left' 71 | pi.velocity(255,255) 72 | pi.right() 73 | if keys[K_RIGHT]: 74 | x=1 75 | #print 'right' 76 | pi.velocity(255,255) 77 | pi.left() 78 | if keys[K_UP]: 79 | x=1 80 | #print 'up' 81 | pi.forward() 82 | pi.velocity(255,255) 83 | if keys[K_DOWN]: 84 | x=1 85 | #print 'down' 86 | pi.velocity(255,255) 87 | pi.back() 88 | if keys[K_SPACE]: 89 | pi.stop(); 90 | break 91 | elif x==0: 92 | pi.stop(); 93 | x=0 94 | #pi.servo_1(degrees1) 95 | #pi.servo_2(degrees2) 96 | 97 | 98 | # 2 - Distance travelled Test 99 | if y == '2': 100 | pi.forward_mm(1000) # distance in mm. maximum allowable value for distanceinmm = 65535 101 | time.sleep(2) 102 | pi.back_mm(300) 103 | time.sleep(1) 104 | 105 | # 3 - Servo motor control Test 106 | if y == '3': 107 | pi.servo_1(0) 108 | pi.servo_2(0) 109 | time.sleep(0.5) # delay is mandatory to ensure motion is complete before next rotation degree is sent 110 | 111 | pi.servo_1(120) 112 | pi.servo_2(120) 113 | 114 | time.sleep(0.5) 115 | 116 | pi.servo_1_free() 117 | pi.servo_2_free() 118 | 119 | #4 - BarLED 120 | if y == '4': 121 | pi.led_bargraph_on(pi.barLED1) 122 | time.sleep(1) 123 | pi.led_bargraph_on(pi.barLED1|pi.barLED2) 124 | time.sleep(1) 125 | pi.led_bargraph_on(pi.barLED1|pi.barLED2|pi.barLED3) 126 | time.sleep(1) 127 | pi.led_bargraph_on(pi.barLED1|pi.barLED2|pi.barLED3|pi.barLED4) 128 | time.sleep(1) 129 | pi.led_bargraph_on(pi.barLED1|pi.barLED2|pi.barLED3|pi.barLED4|pi.barLED5) 130 | time.sleep(1) 131 | pi.led_bargraph_on(pi.barLED1|pi.barLED2|pi.barLED3|pi.barLED4|pi.barLED5|pi.barLED6) 132 | time.sleep(1) 133 | pi.led_bargraph_on(pi.barLED1|pi.barLED2|pi.barLED3|pi.barLED4|pi.barLED5|pi.barLED6|pi.barLED7) 134 | time.sleep(1) 135 | pi.led_bargraph_on(pi.barLED1|pi.barLED2|pi.barLED3|pi.barLED4|pi.barLED5|pi.barLED6|pi.barLED7|pi.barLED8) 136 | time.sleep(2) 137 | 138 | pi.led_bargraph_off(pi.barLED1|pi.barLED2|pi.barLED3|pi.barLED4|pi.barLED5|pi.barLED6|pi.barLED7|pi.barLED8) 139 | 140 | # 5 - ADC sensors values 141 | if y == '5': 142 | pi.adc_conversion(1) # Channel No-1 of ATmega2560 143 | #time.sleep(1) 144 | pi.adc_conversion(2) # Channel No-2 of ATmega2560 145 | time.sleep(0.1) 146 | pi.adc_conversion(3) # Channel No-3 of ATmega2560 147 | time.sleep(0.1) 148 | pi.adc_conversion(4) # Channel No-4 of ATmega2560 149 | time.sleep(0.1) 150 | pi.adc_conversion(5) # Channel No-5 of ATmega2560 151 | time.sleep(0.1) 152 | pi.adc_conversion(6) # Channel No-6 of ATmega2560 153 | time.sleep(0.1) 154 | pi.adc_conversion(7) # Channel No-7 of ATmega2560 155 | time.sleep(0.1) 156 | pi.adc_conversion(8) # Channel No-8 of ATmega2560 157 | time.sleep(0.1) 158 | pi.spi_master_tx_and_rx(1) # Channel No-1 of ATmega8 159 | pi.spi_master_tx_and_rx(5) # Channel No-5 of ATmega8 160 | time.sleep(0.1) 161 | pi.spi_master_tx_and_rx(6) # Channel No-6 of ATmega8 162 | pi.spi_master_tx_and_rx(7) # Channel No-7 of ATmega8 163 | time.sleep(0.1) 164 | 165 | pi.serial_close() # close serial port 166 | 167 | -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Debug/Firebird_Rpi.hex: -------------------------------------------------------------------------------- 1 | :100000000C9472000C9493000C9493000C94930045 2 | :100010000C9493000C9478010C9453010C9493006D 3 | :100020000C9493000C9493000C9493000C94930004 4 | :100030000C9493000C9493000C9493000C949300F4 5 | :100040000C9493000C9493000C9493000C949300E4 6 | :100050000C9493000C9493000C9493000C949300D4 7 | :100060000C9493000C9493000C9493000C949300C4 8 | :100070000C9493000C9493000C9493000C949300B4 9 | :100080000C9493000C9493000C9493000C949300A4 10 | :100090000C9493000C9493000C9493000C94930094 11 | :1000A0000C9493000C9493000C9493000C94930084 12 | :1000B0000C9493000C9493000C9493000C94930074 13 | :1000C0000C9493000C9493000C9493000C94A7024E 14 | :1000D0000C9493000C9493000C9493000C94930054 15 | :1000E0000C94930011241FBECFEFD1E2DEBFCDBF31 16 | :1000F00000E00CBF12E0A0E0B2E0E2ECFCE000E0C7 17 | :100100000BBF02C007900D92A230B107D9F712E0E1 18 | :10011000A2E0B2E001C01D92A635B107E1F70E944E 19 | :10012000CD040C945F060C9400003B9A439808950C 20 | :1001300081B18F6081B982B1807F82B9EAE0F1E05C 21 | :10014000808188618083EBE0F1E080818861808339 22 | :1001500008956C98749A08956D98759A089510BAD8 23 | :1001600011BA10920701109208010895259A2D9A4C 24 | :100170000895269A2E9A0895279A2F9A08958FEF18 25 | :100180008093040110920501089584B1876084B9B9 26 | :1001900085B1876085B908950E9498000E949500F6 27 | :1001A0000E94A9000E94AC000E94AF000E94B6000D 28 | :1001B0000E94B9000E94BC000E94BF000E94C500BE 29 | :1001C0000895E1EDF0E010821092D00086E0809377 30 | :1001D000D2008FE58093D4001092D50088E9808307 31 | :1001E0000895EAE7F0E0108210927B0080E28093AD 32 | :1001F0007C0080E880BF86E880830895E1E2F1E03A 33 | :1002000010828FEF8093250191E09093240110924A 34 | :1002100029018093280110922B0180932A011092CA 35 | :100220002D0180932C0189EA809320018BE080834B 36 | :100230000895E1E8F0E010828CEF8093850081E082 37 | :100240008093840093E0909389008FEF80938800DF 38 | :1002500090938B0080938A0090938D0080938C0004 39 | :1002600090938700809386008BEA809380001092A1 40 | :1002700082008CE080830895F894EAE6F0E08081C3 41 | :1002800082608083EC9A78940895F894EAE6F0E02E 42 | :10029000808188608083ED9A7894089583E58CBD91 43 | :1002A0001DBC1EBC08951F920F920FB60F92112411 44 | :1002B0008F939F93AF93BF93809104029091050217 45 | :1002C000A0910602B09107020196A11DB11D809375 46 | :1002D000040290930502A0930602B0930702BF9117 47 | :1002E000AF919F918F910F900FBE0F901F90189517 48 | :1002F0001F920F920FB60F9211248F939F93AF937B 49 | :10030000BF938091080290910902A0910A02B091D6 50 | :100310000B020196A11DB11D809308029093090262 51 | :10032000A0930A02B0930B02BF91AF919F918F915E 52 | :100330000F900FBE0F901F901895F8940E94CC005C 53 | :100340000E94E1000E94F1000E94FE000E943C0118 54 | :100350000E9445010E9419010E944E01789408955F 55 | :10036000883018F098E090937B00877080628093CB 56 | :100370007C0010924E02EAE7F0E080818064808386 57 | :10038000808184FFFDCF80917900EAE7F0E09081E1 58 | :100390009061908310927B00089580932801609370 59 | :1003A0002A010895EBE0F1E0808182618083089565 60 | :1003B000439A08954398089586E082B9089589E0A4 61 | :1003C00082B9089585E082B908958AE082B90895D6 62 | :1003D00012B80895BC0180E090E00E94D10525EE9E 63 | :1003E00030ED4AEA50E40E943D050E94A505462FE3 64 | :1003F000572F682F792F1092040210920502109245 65 | :100400000602109207028091040290910502A091C9 66 | :100410000602B0910702481759076A077B0798F749 67 | :100420000E94E8010895CF93DF93EC010E94DC0164 68 | :10043000CE010E94EA01DF91CF910895CF93DF931F 69 | :10044000EC010E94DF01CE010E94EA01DF91CF9111 70 | :10045000089510928900682F70E080E090E00E947B 71 | :10046000D1052BE734E14EEE5FE30E943D0520E02D 72 | :1004700030E04CE052E40E94D9040E94A50560934C 73 | :100480008800089510928B00682F70E080E090E063 74 | :100490000E94D1052BE734E14EEE5FE30E943D055B 75 | :1004A00020E030E04CE052E40E94D9040E94A5050F 76 | :1004B00060938A00089510928D00682F70E080E0AC 77 | :1004C00090E00E94D1052BE734E14EEE5FE30E94FD 78 | :1004D0003D0520E030E04CE052E40E94D9040E9447 79 | :1004E000A50560938C00089583E0809389008FEFC9 80 | :1004F00080938800089583E080938B008FEF809332 81 | :100500008A00089583E080938D008FEF80938C00A4 82 | :100510000895E5E0F1E09081892B80830895E5E07E 83 | :10052000F1E0908189238083089528988EBD0DB4D1 84 | :1005300007FEFDCF86E69EE00197F1F7000080E51B 85 | :100540008EBD0DB407FEFDCF8EB5289A08951F927B 86 | :100550000F920FB60F920BB60F9211242F933F9369 87 | :100560008F939F93AF93BF93EF93FF939091D60098 88 | :1005700090934D02E0EDF0E080818823E9F38091D3 89 | :100580001702882309F047C09A3071F4809119024C 90 | :10059000E4E3F2E0E80FF11D90838F5F809319028E 91 | :1005A00081E080931A0237C080911A02813049F5A8 92 | :1005B0009D3089F520911902E4E3F2E0E20FF11D8C 93 | :1005C0008DE08083322F3F5F3093190282E0809369 94 | :1005D0001A0281E080931702109218023323D9F097 95 | :1005E000EBE1F2E0A4E3B2E0CF010196820F911DAE 96 | :1005F0002C9121931D92E817F907D1F73093180237 97 | :100600000AC080911902E4E3F2E0E80FF11D908343 98 | :100610008F5F80931902FF91EF91BF91AF919F91EE 99 | :100620008F913F912F910F900BBE0F900FBE0F90A7 100 | :100630001F90189580911502882309F06AC0809157 101 | :100640001302882329F40E94B0018093D60061C070 102 | :10065000813029F40E94B0018093D6005AC08230C4 103 | :1006600029F40E94B0018093D60053C0833029F44E 104 | :100670000E94B0018093D6004CC0843029F40E94BF 105 | :10068000B0018093D60045C0853029F40E94B001A6 106 | :100690008093D6003EC0863029F40E94B00180933A 107 | :1006A000D60037C0873029F40E94B0018093D6006D 108 | :1006B00030C0883029F40E94B0018093D60029C050 109 | :1006C000893029F40E94B0018093D60022C08A307C 110 | :1006D00029F40E94B0018093D6001BC08B3029F40E 111 | :1006E0000E94B0018093D60014C08C3029F40E947F 112 | :1006F000B0018093D6000DC08D3029F40E94B00166 113 | :100700008093D60006C08E3021F40E94B001809301 114 | :10071000D600809115028130C9F58091130288239B 115 | :1007200029F40E9495028093D6000895813029F41F 116 | :100730000E9495028093D6000895823029F40E9489 117 | :1007400095028093D6000895833029F40E94950283 118 | :100750008093D6000895843029F40E9495028093F6 119 | :10076000D6000895853029F40E9495028093D60022 120 | :100770000895863029F40E9495028093D60008954A 121 | :10078000873021F40E9495028093D60008958091CD 122 | :100790001502813059F480911302882319F40E94C4 123 | :1007A000D80104C0813011F40E94DA018091150251 124 | :1007B000823069F580911302882329F40E94D201C6 125 | :1007C0000E94DC0124C0813029F40E94D2010E94E1 126 | :1007D000DF011DC0823029F40E94D2010E94E50190 127 | :1007E00016C0833029F40E94D2010E94E2010FC09A 128 | :1007F000843029F40E94D2010E94E80108C08930A7 129 | :1008000031F48091100260910E020E94CD0180911E 130 | :1008100015028330A1F480911302882331F4809172 131 | :100820001002909111020E941302809113028130F4 132 | :1008300031F480911002909111020E941E02809169 133 | :100840001502843051F580911302882321F48091A0 134 | :1008500010020E94290280911302813021F48091BC 135 | :1008600010020E94420280911302823021F4809192 136 | :1008700010020E945B0280911302833011F40E94E7 137 | :10088000740280911302843011F40E947B028091E3 138 | :100890001302853011F40E94820280911502853086 139 | :1008A00081F480911302882321F4809110020E9428 140 | :1008B000890280911302813021F4809110020E94FC 141 | :1008C0008F02089580911602813009F046C0809110 142 | :1008D0001B028093150280911C0280931402809168 143 | :1008E0001D028093130280911E0280931202982FA2 144 | :1008F000907190935202982F9F709093540284FFAE 145 | :1009000011C020911F022093550230912002309394 146 | :100910005102932F80E0820F911D909311028093DA 147 | :10092000100218C0913019F09230A1F407C08091E4 148 | :100930001F0280931002109211020CC080911F02BE 149 | :1009400080931002109211028091200280930E0277 150 | :1009500010920F021092160202C0882361F4809157 151 | :100960001402882331F40E941A0308950E94C703D9 152 | :1009700008958130D9F3089580911702813061F490 153 | :1009800010921902109218021092170210921A0275 154 | :10099000809316020E94620408950E949D0180E0E7 155 | :1009A0000E94420280E00E945B020E94BC04FDCFD4 156 | :1009B0005058BB27AA270ED048C139D130F03ED1BC 157 | :1009C00020F031F49F3F11F41EF42EC10EF4E09597 158 | :1009D000E7FB24C1E92F4AD180F3BA1762077307F6 159 | :1009E0008407950718F071F49EF562C10EF4E09546 160 | :1009F0000B2EBA2FA02D0B01B90190010C01CA01D9 161 | :100A0000A0011124FF27591B99F0593F50F4503E83 162 | :100A100068F11A16F040A22F232F342F4427585F75 163 | :100A2000F3CF469537952795A795F0405395C9F78D 164 | :100A30007EF41F16BA0B620B730B840BBAF0915045 165 | :100A4000A1F0FF0FBB1F661F771F881FC2F70EC0E4 166 | :100A5000BA0F621F731F841F48F4879577956795B7 167 | :100A6000B795F7959E3F08F0B3CF9395880F08F0A0 168 | :100A70009927EE0F9795879508950CD0E6C0DED0A4 169 | :100A800040F0D5D030F021F45F3F19F0C7C05111CC 170 | :100A900010C1CAC0EBD098F39923C9F35523B1F321 171 | :100AA000951B550BBB27AA2762177307840738F0DD 172 | :100AB0009F5F5F4F220F331F441FAA1FA9F333D03C 173 | :100AC0000E2E3AF0E0E830D091505040E695001CF0 174 | :100AD000CAF729D0FE2F27D0660F771F881FBB1FAC 175 | :100AE000261737074807AB07B0E809F0BB0B802D86 176 | :100AF000BF01FF2793585F4F2AF09E3F510568F0D2 177 | :100B00008DC0D7C05F3FECF3983EDCF386957795B8 178 | :100B10006795B795F7959F5FC9F7880F911D9695D3 179 | :100B2000879597F90895E1E0660F771F881FBB1F2F 180 | :100B3000621773078407BA0720F0621B730B840BDC 181 | :100B4000BA0BEE1F88F7E095089598D088F09F576C 182 | :100B500090F0B92F9927B751A0F0D1F0660F771F09 183 | :100B6000881F991F1AF0BA95C9F712C0B13081F0E9 184 | :100B70009FD0B1E008959CC0672F782F8827B85F79 185 | :100B800039F0B93FCCF3869577956795B395D9F74A 186 | :100B90003EF490958095709561957F4F8F4F9F4F54 187 | :100BA0000895E89409C097FB3EF490958095709560 188 | :100BB00061957F4F8F4F9F4F9923A9F0F92F96E9A9 189 | :100BC000BB279395F695879577956795B795F1111E 190 | :100BD000F8CFFAF4BB0F11F460FF1BC06F5F7F4FBB 191 | :100BE0008F4F9F4F16C0882311F096E911C07723CD 192 | :100BF00021F09EE8872F762F05C0662371F096E8D6 193 | :100C0000862F70E060E02AF09A95660F771F881FA4 194 | :100C1000DAF7880F9695879597F9089597F99F6762 195 | :100C200080E870E060E008959FEF80EC0895002474 196 | :100C30000A941616170618060906089500240A9441 197 | :100C400012161306140605060895092E0394000CC7 198 | :100C500011F4882352F0BB0F40F4BF2B11F460FF56 199 | :100C600004C06F5F7F4F8F4F9F4F089557FD90587F 200 | :100C7000440F551F59F05F3F71F04795880F97FB60 201 | :100C8000991F61F09F3F79F087950895121613061A 202 | :100C90001406551FF2CF4695F1DF08C01616170649 203 | :100CA0001806991FF1CF869571056105089408957E 204 | :100CB000E894BB2766277727CB0197F90895F89426 205 | :020CC000FFCF64 206 | :020CC200200010 207 | :00000001FF 208 | -------------------------------------------------------------------------------- /Surrogates/Python-Code/Python-Code-version1.0/line-follower.py: -------------------------------------------------------------------------------- 1 | import serial 2 | import time 3 | import glob 4 | import logging 5 | ########### Variable declaration 6 | global device_id 7 | global device_type 8 | global function_type 9 | global param_count 10 | global port 11 | #global wl_threshold 12 | wl_threshold = 20 13 | sharp_threshold = 100 14 | ir_threshold = 95 15 | left_sensor_value = 0 16 | centre_sensor_value = 0 17 | right_sensor_value = 0 18 | front_sharp_value = 0 19 | front_ir_value = 0 20 | 21 | port_detect = glob.glob("/dev/ttyUSB*") # stores all /dev/ttyUSB* into a list port_detect 22 | 23 | def serial_port_connection(): 24 | global port 25 | 26 | print len(port_detect),"Ports detected" # print number of ports detected 27 | 28 | #------------- print all detectec ports - STARTS ---------# 29 | if (len(port_detect) != 0): 30 | print "Port(s) detected is/are:" 31 | 32 | for i in range (0,len(port_detect)): 33 | print port_detect[i] 34 | #------------- print all detectec ports - END ---------# 35 | 36 | #------------- connect to PORT if only one port is detected - STARTS ---------# 37 | if (len(port_detect) == 1): 38 | port = serial.Serial(port_detect[0],baudrate=9600) 39 | print "connected to: ", port_detect[0] 40 | #------------- connect to PORT if only one port is detected - END ---------# 41 | 42 | #------------- Ask for user i/p if more then one port is detected - STARTS ---------# 43 | else: 44 | for i in range(0,len(port_detect)): 45 | print "Enter",i,"to connect to:",port_detect[i] 46 | 47 | y = int(raw_input("Enter your choice of connection: ")) 48 | 49 | while y >= len(port_detect): 50 | print "Invalid choice" 51 | for i in range(0,len(port_detect)): 52 | print "Enter",i,"to connect to:",port_detect[i] 53 | y = int(raw_input("Enter your choice of connection: ")) 54 | #------------- Ask for user i/p if more then one port is detected - END ---------# 55 | 56 | port = serial.Serial(port_detect[y],baudrate=9600) 57 | print "connected to: ", port_detect[y] 58 | return 59 | 60 | try: 61 | serial_port_connection() 62 | 63 | if port.isOpen() == True: 64 | print "Port is open" 65 | else: 66 | serial_port_connection() 67 | 68 | except: 69 | print "No USB port detected....check connection" 70 | sys.exit(0) # stop program execution when exception occur 71 | 72 | def assemble_data (device_id,device_type,function_type,param_count): 73 | 74 | return data 75 | 76 | def forward (): 77 | print "In forward" 78 | data = [] 79 | device_id = 2 #DC Motors has device id = 2 80 | device_type = 1 #DC Motors is o/p device. hence device type = 0 81 | function_type = 0 #Function_type = 0 for forward motion 82 | param_count = 0 #No parameter is sent through forward function hence param_count = 0 83 | data.append(chr(device_id)) 84 | data.append(chr(device_type)) 85 | data.append(chr(function_type)) 86 | data.append(chr(param_count)) 87 | data.append("\n") 88 | data.append("\r") 89 | 90 | for i in range(0,len(data)): 91 | port.write(str(data[i])) 92 | print str(data[i]) 93 | #port.write(str(device_id)) 94 | #print "device_id =", device_id 95 | #port.write(str(device_type)) 96 | #port.write(str(function_type)) 97 | #port.write(str(param_count)) 98 | #port.write("\n") 99 | #port.write("\r") 100 | print "packet sent is" , str(data) 101 | return 102 | 103 | def stop (): 104 | print "in Stop" 105 | device_id = 2 106 | device_type = 1 107 | function_type = 4 108 | param_count = 0 109 | port.write(chr(device_id)) 110 | port.write(chr(device_type)) 111 | port.write(chr(function_type)) 112 | print "function_type =",function_type 113 | port.write(chr(param_count)) 114 | port.write("\n") 115 | port.write("\r") 116 | return 117 | 118 | def buzzer_on(): 119 | device_id = 1 120 | device_type = 1 121 | function_type = 0 122 | param_count = 0 123 | port.write(chr(device_id)) 124 | port.write(chr(device_type)) 125 | port.write(chr(function_type)) 126 | port.write(chr(param_count)) 127 | port.write("\n") 128 | port.write("\r") 129 | return 130 | 131 | def buzzer_off(): 132 | device_id = 1 133 | device_type = 1 134 | function_type = 1 135 | param_count = 0 136 | 137 | port.write(chr(device_id)) 138 | port.write(chr(device_type)) 139 | port.write(chr(function_type)) 140 | port.write(chr(param_count)) 141 | port.write("\n") 142 | port.write("\r") 143 | #print "buzzer off" 144 | return 145 | 146 | def velocity(left_motor,right_motor): 147 | data = [] 148 | device_id = 2 149 | device_type = 1 150 | function_type = 9 151 | param_count = 2 152 | param_1 = left_motor 153 | param_2 = right_motor 154 | data.append(chr(device_id)) 155 | data.append(chr(device_type)) 156 | data.append(chr(function_type)) 157 | data.append(chr(param_count)) 158 | data.append(chr(param_1)) 159 | data.append(chr(param_2)) 160 | data.append("\n") 161 | data.append("\r") 162 | 163 | for i in range(0,len(data)): 164 | port.write(str(data[i])) 165 | #print str(data[i]) 166 | print "packet sent is ", str(data) 167 | return 168 | 169 | def adc_conversion(channel_no): 170 | data = [] 171 | device_id = channel_no 172 | device_type = 0 # sensors are input so device_type is 0 173 | function_type = 0 174 | param_count = 0 175 | data.append(chr(device_id)) 176 | data.append(chr(device_type)) 177 | data.append(chr(function_type)) 178 | data.append(chr(param_count)) 179 | data.append("\n") 180 | data.append("\r") 181 | 182 | for i in range(0,len(data)): 183 | port.write(str(data[i])) 184 | #print str(data[i]) 185 | print "packet sent is ", str(data) 186 | 187 | ret = port.read() 188 | print "channel no:",channel_no, "returned: ", '%s' %str(ord(ret)) 189 | 190 | return '%s' %str(ord(ret)) 191 | 192 | while True: 193 | 194 | y= raw_input("Enter the number:") 195 | if y == '0': 196 | stop() 197 | print "good bye" 198 | break 199 | if y == '1': 200 | print "test running" 201 | forward() 202 | time.sleep(5) 203 | stop() 204 | time.sleep(2) 205 | 206 | #buzzer_on() 207 | #time.sleep(1) 208 | #buzzer_off() 209 | #time.sleep(1) 210 | 211 | velocity(255,255) 212 | print "velocity set to 255,255" 213 | ret = port.read() 214 | print "value returned by bot is ", ret 215 | time.sleep(3) 216 | velocity(255,200) 217 | print "velocity set to 200,200" 218 | time.sleep(5) 219 | velocity(0,0) 220 | print "velocity set to 0,0" 221 | time.sleep(3) 222 | velocity(255,255) 223 | print "velocity set to 255,255" 224 | time.sleep(3) 225 | 226 | #----------------------- Line following ----------------------# 227 | if y == '2': 228 | while (1): 229 | print "wl_threshold set to: ",wl_threshold 230 | left_sensor_value = int(adc_conversion(3)) 231 | centre_sensor_value = int(adc_conversion(2)) 232 | right_sensor_value = int(adc_conversion(1)) 233 | #time.sleep(1) # for debugging purpose 234 | 235 | # ''' 236 | if ((left_sensor_value < wl_threshold) and (centre_sensor_value < wl_threshold) and (right_sensor_value < wl_threshold)): # all 3 on white 237 | print "all on white" 238 | velocity(0,0) # Stop 239 | 240 | if ((left_sensor_value > wl_threshold) and (centre_sensor_value < wl_threshold) and (right_sensor_value < wl_threshold)): # Right & centre on white - left deviation 241 | print "right and centre on white" 242 | velocity(230,200) # right turn 243 | 244 | if ((left_sensor_value > wl_threshold) and (centre_sensor_value > wl_threshold) and (right_sensor_value < wl_threshold)): # Right on white - more left deviation 245 | print "right on white" 246 | velocity(255,180) # right turn 247 | 248 | if ((left_sensor_value < wl_threshold) and (centre_sensor_value < wl_threshold) and (right_sensor_value > wl_threshold)): # Left & centre on white - right deviation 249 | print "left and centre on white" 250 | velocity(200,230) # left turn 251 | 252 | if ((left_sensor_value < wl_threshold) and (centre_sensor_value > wl_threshold) and (right_sensor_value > wl_threshold)): # Left on white - more right deviation 253 | print "left on white" 254 | velocity(180,255) # left turn 255 | 256 | if ((left_sensor_value > wl_threshold) and (centre_sensor_value < wl_threshold) and (right_sensor_value > wl_threshold)): # centre on white - no deviation 257 | print "centre on white" 258 | velocity(255,255) # forward 259 | 260 | if ((left_sensor_value > wl_threshold) and (centre_sensor_value > wl_threshold) and (right_sensor_value > wl_threshold)): # no 3 on white 261 | print "all on black" 262 | velocity(0,0) # Stop 263 | 264 | time.sleep(0.5) 265 | port.flushInput() 266 | port.flushOutput() 267 | 268 | #adc_conversion(1) # R WL 269 | #adc_conversion(2) # C WL 270 | #adc_conversion(3) # L WL 271 | 272 | #adc_conversion(4) 273 | #adc_conversion(5) 274 | #adc_conversion(6) 275 | 276 | # ''' 277 | 278 | #----------------------- Adaptive cruise control ----------------------# 279 | if y == '3': 280 | print "Line threshold set to: ",wl_threshold 281 | print "Sharp threshold set to: ",sharp_threshold 282 | print "IR threshold set to: ",ir_threshold 283 | while (1): 284 | left_sensor_value = int(adc_conversion(3)) 285 | centre_sensor_value = int(adc_conversion(2)) 286 | right_sensor_value = int(adc_conversion(1)) 287 | 288 | front_sharp_value = int(adc_conversion(11)) 289 | front_ir_value = int(adc_conversion(6)) 290 | 291 | buzzer_off() 292 | #time.sleep(1) 293 | flag_object = 0 # goes high when object is detected in-front of robot 294 | 295 | 296 | if ((front_sharp_value > sharp_threshold) or (front_ir_value < ir_threshold)): 297 | print "Object detected" 298 | flag_object = 1 299 | velocity(0,0) # Stop 300 | buzzer_on() 301 | #time.sleep(1) 302 | #else: 303 | # print "not in if" 304 | # buzzer_off() 305 | 306 | # """ 307 | if ((left_sensor_value < wl_threshold) and (centre_sensor_value < wl_threshold) and (right_sensor_value < wl_threshold) and (flag_object == 0)): # all 3 on white 308 | print "all on white" 309 | velocity(0,0) # Stop 310 | buzzer_off() 311 | 312 | if ((left_sensor_value > wl_threshold) and (centre_sensor_value < wl_threshold) and (right_sensor_value < wl_threshold) and (flag_object == 0)): # Right & centre on white - left deviation 313 | print "right and centre on white" 314 | velocity(230,200) # right turn 315 | buzzer_off() 316 | 317 | if ((left_sensor_value > wl_threshold) and (centre_sensor_value > wl_threshold) and (right_sensor_value < wl_threshold) and (flag_object == 0)): # Right on white - more left deviation 318 | print "right on white" 319 | velocity(255,180) # right turn 320 | buzzer_off() 321 | 322 | if ((left_sensor_value < wl_threshold) and (centre_sensor_value < wl_threshold) and (right_sensor_value > wl_threshold) and (flag_object == 0)): # Left & centre on white - right deviation 323 | print "left and centre on white" 324 | velocity(200,230) # left turn 325 | buzzer_off() 326 | 327 | if ((left_sensor_value < wl_threshold) and (centre_sensor_value > wl_threshold) and (right_sensor_value > wl_threshold) and (flag_object == 0)): # Left on white - more right deviation 328 | print "left on white" 329 | velocity(180,255) # left turn 330 | buzzer_off() 331 | 332 | if ((left_sensor_value > wl_threshold) and (centre_sensor_value < wl_threshold) and (right_sensor_value > wl_threshold) and (flag_object == 0)): # centre on white - no deviation 333 | print "centre on white" 334 | velocity(255,255) # forward 335 | buzzer_off() 336 | 337 | if ((left_sensor_value > wl_threshold) and (centre_sensor_value > wl_threshold) and (right_sensor_value > wl_threshold) and (flag_object == 0)): # no 3 on white 338 | print "all on black" 339 | velocity(0,0) # Stop 340 | buzzer_off() 341 | # ''' 342 | time.sleep(0.5) 343 | port.flushInput() 344 | port.flushOutput() 345 | 346 | #adc_conversion(1) # R WL 347 | #adc_conversion(2) # C WL 348 | #adc_conversion(3) # L WL 349 | 350 | #adc_conversion(4) 351 | #adc_conversion(5) 352 | #adc_conversion(6) 353 | 354 | 355 | #port.flushOutput() 356 | #ret = port.read() 357 | #print "data returned by bot = ", ret 358 | 359 | 360 | 361 | print "out of the loop" 362 | port.close() 363 | -------------------------------------------------------------------------------- /Surrogates/Python-Code/Python-Code-version1.0/fipi.py: -------------------------------------------------------------------------------- 1 | ''' 2 | 3 | Created: 23-04-2015 4 | Last Modified: 30-03-2016 5 | Author: Original: Saurav Shandilya (e-Yantra Team) [Modified: Ganesh Iyer] 6 | Application: Development of Raspberry Pi controlled Firebird-V Robot. 7 | Hardware: Firebird-V Atmega2560 Robot and Raspberry Pi 2 8 | Python Version: 2.7 9 | 10 | ---------------------------------- 11 | Changes: For left and right keyboard/ keypress control 12 | 13 | ''' 14 | 15 | #***********************Library Import Starts********************* 16 | import serial 17 | import time 18 | import glob # Glob module finds all the pathnames matching a specified pattern. It is used for detecting serial ports in use 19 | import sys # This module provides access to some variables used or maintained by the interpreter. It is used to exit from program when exception occur 20 | 21 | #***********************Library Import Ends********************* 22 | 23 | 24 | #**********************Variable declaration Starts********************* 25 | global device_id 26 | global device_type 27 | global function_type 28 | global param_count 29 | global port 30 | global port_detect 31 | barLED1 = 1 32 | barLED2 = 2 33 | barLED3 = 4 34 | barLED4 = 8 35 | barLED5 = 16 36 | barLED6 = 32 37 | barLED7 = 64 38 | barLED8 = 128 39 | 40 | #**********************Variable declaration Ends********************* 41 | 42 | #**********************Communication/Serial Port Detection Starts********************* 43 | def serial_port_connection(port_detect): 44 | global port 45 | 46 | print len(port_detect),"Ports detected" # print number of ports detected 47 | 48 | #------------- print all detectec ports - STARTS ---------# 49 | if (len(port_detect) != 0): 50 | print "Port(s) detected is/are:" 51 | 52 | for i in range (0,len(port_detect)): 53 | print port_detect[i] 54 | #------------- print all detectec ports - END ---------# 55 | 56 | #------------- connect to PORT if only one port is detected - STARTS ---------# 57 | if (len(port_detect) == 1): 58 | port = serial.Serial(port_detect[0],baudrate=9600) 59 | print "connected to: ", port_detect[0] 60 | #------------- connect to PORT if only one port is detected - END ---------# 61 | 62 | #------------- Ask for user i/p if more then one port is detected - STARTS ---------# 63 | else: 64 | for i in range(0,len(port_detect)): 65 | print "Enter",i,"to connect to:",port_detect[i] 66 | 67 | y = int(raw_input("Enter your choice of connection: ")) 68 | 69 | while y >= len(port_detect): 70 | print "Invalid choice" 71 | for i in range(0,len(port_detect)): 72 | print "Enter",i,"to connect to:",port_detect[i] 73 | y = int(raw_input("Enter your choice of connection: ")) 74 | #------------- Ask for user i/p if more then one port is detected - END ---------# 75 | 76 | port = serial.Serial(port_detect[y],baudrate=9600) 77 | print "connected to: ", port_detect[y] 78 | return 79 | #**********************Communication/Serial Port Detection Ends********************* 80 | 81 | #**********************Open Communication/Serial Port Starts********************* 82 | def serial_open(): 83 | port_detect = glob.glob("/dev/ttyUSB*") # stores all /dev/ttyUSB* into a list port_detect 84 | 85 | try: 86 | serial_port_connection(port_detect) 87 | 88 | if port.isOpen() == True: 89 | print "Port is open" 90 | else: 91 | serial_port_connection() 92 | 93 | except: 94 | print "No USB port detected....check connection" 95 | sys.exit(0) # stop program execution when exception occur 96 | #**********************Open Communication/Serial Port Starts********************* 97 | 98 | 99 | #**********************Close Communication/Serial Port Starts********************* 100 | def serial_close(): 101 | port.close() 102 | #**********************Close Communication/Serial Port Ends********************* 103 | 104 | 105 | #**********************Forward Starts********************* 106 | def forward (): 107 | print "Forward Motion" 108 | data = [] 109 | device_id = 2 #DC Motors has device id = 2 110 | device_type = 1 #DC Motors is o/p device. hence device type = 0 111 | function_type = 0 #Function_type = 0 for forward motion 112 | param_count = 0 #No parameter is sent through forward function hence param_count = 0 113 | data.append(chr(device_id)) 114 | data.append(chr(device_type)) 115 | data.append(chr(function_type)) 116 | data.append(chr(param_count)) 117 | data.append("\n") 118 | data.append("\r") 119 | 120 | for i in range(0,len(data)): 121 | port.write(str(data[i])) 122 | print str(data[i]) 123 | #port.write(str(device_id)) 124 | #print "device_id =", device_id 125 | #port.write(str(device_type)) 126 | #port.write(str(function_type)) 127 | #port.write(str(param_count)) 128 | #port.write("\n") 129 | #port.write("\r") 130 | print "packet sent is" , str(data) 131 | return 132 | #**********************Forward Ends********************* 133 | 134 | #**********************Back Starts********************* 135 | def back (): 136 | print "Back Motion" 137 | data = [] 138 | device_id = 2 #DC Motors has device id = 2 139 | device_type = 1 #DC Motors is o/p device. hence device type = 0 140 | function_type = 1 #Function_type = 0 for forward motion 141 | param_count = 0 #No parameter is sent through forward function hence param_count = 0 142 | data.append(chr(device_id)) 143 | data.append(chr(device_type)) 144 | data.append(chr(function_type)) 145 | data.append(chr(param_count)) 146 | data.append("\n") 147 | data.append("\r") 148 | 149 | for i in range(0,len(data)): 150 | port.write(str(data[i])) 151 | print str(data[i]) 152 | #port.write(str(device_id)) 153 | #print "device_id =", device_id 154 | #port.write(str(device_type)) 155 | #port.write(str(function_type)) 156 | #port.write(str(param_count)) 157 | #port.write("\n") 158 | #port.write("\r") 159 | print "packet sent is" , str(data) 160 | return 161 | #**********************Back Starts********************* 162 | 163 | #**********************Left Starts********************* 164 | def left (): 165 | print "Left Motion" 166 | data = [] 167 | device_id = 2 #DC Motors has device id = 2 168 | device_type = 1 #DC Motors is o/p device. hence device type = 0 169 | function_type = 2 #Function_type = 0 for forward motion 170 | param_count = 0 #No parameter is sent through forward function hence param_count = 0 171 | data.append(chr(device_id)) 172 | data.append(chr(device_type)) 173 | data.append(chr(function_type)) 174 | data.append(chr(param_count)) 175 | data.append("\n") 176 | data.append("\r") 177 | 178 | for i in range(0,len(data)): 179 | port.write(str(data[i])) 180 | print str(data[i]) 181 | #port.write(str(device_id)) 182 | #print "device_id =", device_id 183 | #port.write(str(device_type)) 184 | #port.write(str(function_type)) 185 | #port.write(str(param_count)) 186 | #port.write("\n") 187 | #port.write("\r") 188 | print "packet sent is" , str(data) 189 | return 190 | #**********************Left Ends********************* 191 | 192 | #**********************Right Starts********************* 193 | def right (): 194 | print "Right Motion" 195 | data = [] 196 | device_id = 2 #DC Motors has device id = 2 197 | device_type = 1 #DC Motors is o/p device. hence device type = 0 198 | function_type = 3 #Function_type = 0 for forward motion 199 | param_count = 0 #No parameter is sent through forward function hence param_count = 0 200 | data.append(chr(device_id)) 201 | data.append(chr(device_type)) 202 | data.append(chr(function_type)) 203 | data.append(chr(param_count)) 204 | data.append("\n") 205 | data.append("\r") 206 | 207 | for i in range(0,len(data)): 208 | port.write(str(data[i])) 209 | print str(data[i]) 210 | #port.write(str(device_id)) 211 | #print "device_id =", device_id 212 | #port.write(str(device_type)) 213 | #port.write(str(function_type)) 214 | #port.write(str(param_count)) 215 | #port.write("\n") 216 | #port.write("\r") 217 | print "packet sent is" , str(data) 218 | return 219 | #**********************Right Ends********************* 220 | 221 | #**********************Stop Starts********************* 222 | def stop (): 223 | print "Stopping DC Motors" 224 | device_id = 2 225 | device_type = 1 226 | function_type = 4 227 | param_count = 0 228 | port.write(chr(device_id)) 229 | port.write(chr(device_type)) 230 | port.write(chr(function_type)) 231 | port.write(chr(param_count)) 232 | port.write("\n") 233 | port.write("\r") 234 | return 235 | #**********************Stop Ends********************* 236 | 237 | #**********************Buzzer On Starts********************* 238 | def buzzer_on(): 239 | print "Buzzer On" 240 | device_id = 1 241 | device_type = 1 242 | function_type = 0 243 | param_count = 0 244 | port.write(chr(device_id)) 245 | port.write(chr(device_type)) 246 | port.write(chr(function_type)) 247 | port.write(chr(param_count)) 248 | port.write("\n") 249 | port.write("\r") 250 | return 251 | #**********************Buzzer On Ends********************* 252 | 253 | #**********************Buzzer Off Starts********************* 254 | def buzzer_off(): 255 | print "Buzzer OFF" 256 | device_id = 1 257 | device_type = 1 258 | function_type = 1 259 | param_count = 0 260 | 261 | port.write(chr(device_id)) 262 | port.write(chr(device_type)) 263 | port.write(chr(function_type)) 264 | port.write(chr(param_count)) 265 | port.write("\n") 266 | port.write("\r") 267 | return 268 | #**********************Buzzer Off Ends********************* 269 | 270 | #**********************Velocity Control Starts********************* 271 | def velocity(left_motor,right_motor): 272 | print "Left motor velocity = ", '%s' %str(left_motor), "Right motor velocity = ",'%s' %str(right_motor) 273 | data = [] 274 | device_id = 2 275 | device_type = 1 276 | function_type = 9 277 | param_count = 2 278 | param_1 = left_motor 279 | param_2 = right_motor 280 | data.append(chr(device_id)) 281 | data.append(chr(device_type)) 282 | data.append(chr(function_type)) 283 | data.append(chr(param_count)) 284 | data.append(chr(param_1)) 285 | data.append(chr(param_2)) 286 | data.append("\n") 287 | data.append("\r") 288 | 289 | for i in range(0,len(data)): 290 | port.write(str(data[i])) 291 | print str(data[i]) 292 | print "packet sent is ", str(data) 293 | return 294 | #**********************Velocity Control Ends********************* 295 | 296 | #**********************Forward distance travelled Starts********************* 297 | def forward_mm(distanceinmm): # maximum allowable value for distanceinmm = 65535 298 | print "Forward motion for ", "%s" %str(distanceinmm), "mm" 299 | data = [] 300 | device_id = 3 301 | device_type = 1 302 | function_type = 0 303 | param_count = 1 304 | param_1 = distanceinmm 305 | data.append(chr(device_id)) 306 | data.append(chr(device_type)) 307 | data.append(chr(function_type)) 308 | if distanceinmm > 255: 309 | param_1_1 = distanceinmm%256 310 | param_1_2 = distanceinmm/256 311 | data.append(chr(param_count + 16)) # adding 16 = 0x10 => if value to be sent over UART > 8 bits 312 | data.append(chr(param_1_1)) 313 | data.append(chr(param_1_2)) 314 | else: 315 | param_1 = distanceinmm 316 | data.append(chr(param_count)) 317 | data.append(chr(param_1)) 318 | data.append("\n") 319 | data.append("\r") 320 | 321 | for i in range(0,len(data)): 322 | port.write(str(data[i])) 323 | print str(data[i]) 324 | print "packet sent is ", str(data) 325 | return 326 | #**********************Forward distance travelled Ends********************* 327 | 328 | #**********************Backward distance travelled Starts********************* 329 | def back_mm(distanceinmm): # maximum allowable value for distanceinmm = 65535 330 | print "Backward motion for ", "%s" %str(distanceinmm), "mm" 331 | data = [] 332 | device_id = 3 333 | device_type = 1 334 | function_type = 1 335 | param_count = 1 336 | param_1 = distanceinmm 337 | data.append(chr(device_id)) 338 | data.append(chr(device_type)) 339 | data.append(chr(function_type)) 340 | if distanceinmm > 255: 341 | param_1_1 = distanceinmm%256 342 | param_1_2 = distanceinmm/256 343 | data.append(chr(param_count + 16)) # adding 16 = 0x10 => if value to be sent over UART > 8 bits 344 | data.append(chr(param_1_1)) 345 | data.append(chr(param_1_2)) 346 | else: 347 | param_1 = distanceinmm 348 | data.append(chr(param_count)) 349 | data.append(chr(param_1)) 350 | data.append("\n") 351 | data.append("\r") 352 | 353 | for i in range(0,len(data)): 354 | port.write(str(data[i])) 355 | print str(data[i]) 356 | print "packet sent is ", str(data) 357 | return 358 | #**********************Backward distance travelled Ends********************* 359 | 360 | #**********************ADC Conversion Starts********************* 361 | def adc_conversion(channel_no): 362 | data = [] 363 | device_id = 0 364 | device_type = 0 # sensors are input so device_type is 0 365 | function_type = channel_no 366 | param_count = 0 367 | data.append(chr(device_id)) 368 | data.append(chr(device_type)) 369 | data.append(chr(function_type)) 370 | data.append(chr(param_count)) 371 | data.append("\n") 372 | data.append("\r") 373 | 374 | for i in range(0,len(data)): 375 | port.write(str(data[i])) 376 | #print str(data[i]) 377 | print "packet sent is ", str(data) 378 | ret = port.read() 379 | print "channel no:",channel_no, "returned: ", '%s' %str(ord(ret)) 380 | port.flushInput() 381 | port.flushOutput() 382 | return 383 | #**********************ADC Conversion Ends********************* 384 | 385 | #**********************SPI Starts********************* 386 | def spi_master_tx_and_rx(channel_no): 387 | data = [] 388 | device_id = 1 389 | device_type = 0 # sensors are input so device_type is 0 390 | function_type = channel_no 391 | param_count = 0 392 | data.append(chr(device_id)) 393 | data.append(chr(device_type)) 394 | data.append(chr(function_type)) 395 | data.append(chr(param_count)) 396 | data.append("\n") 397 | data.append("\r") 398 | 399 | for i in range(0,len(data)): 400 | port.write(str(data[i])) 401 | #print str(data[i]) 402 | print "packet sent is ", str(data) 403 | 404 | ret = port.read() 405 | print "spi_channel no:",channel_no, "returned: ", '%s' %str(ord(ret)) 406 | port.flushInput() 407 | port.flushOutput() 408 | return 409 | #**********************SPI Ends********************* 410 | 411 | #**********************Servo Control Starts********************* 412 | def servo_1(degree): 413 | data = [] 414 | if degree > 180: 415 | degree = 180 416 | device_id = 4 417 | device_type = 1 418 | function_type = 0 419 | param_count = 1 420 | param_1 = degree 421 | data.append(chr(device_id)) 422 | data.append(chr(device_type)) 423 | data.append(chr(function_type)) 424 | data.append(chr(param_count)) 425 | data.append(chr(param_1)) 426 | data.append("\n") 427 | data.append("\r") 428 | 429 | for i in range(0,len(data)): 430 | port.write(str(data[i])) 431 | print str(data[i]) 432 | print "packet sent is ", str(data) 433 | 434 | def servo_2(degree): 435 | data = [] 436 | if degree > 180: 437 | degree = 180 438 | device_id = 4 439 | device_type = 1 440 | function_type = 1 441 | param_count = 1 442 | param_1 = degree 443 | data.append(chr(device_id)) 444 | data.append(chr(device_type)) 445 | data.append(chr(function_type)) 446 | data.append(chr(param_count)) 447 | data.append(chr(param_1)) 448 | data.append("\n") 449 | data.append("\r") 450 | 451 | for i in range(0,len(data)): 452 | port.write(str(data[i])) 453 | print str(data[i]) 454 | print "packet sent is ", str(data) 455 | 456 | def servo_3(degree): 457 | data = [] 458 | if degree > 180: 459 | degree = 180 460 | device_id = 4 461 | device_type = 1 462 | function_type = 2 463 | param_count = 1 464 | param_1 = degree 465 | data.append(chr(device_id)) 466 | data.append(chr(device_type)) 467 | data.append(chr(function_type)) 468 | data.append(chr(param_count)) 469 | data.append(chr(param_1)) 470 | data.append("\n") 471 | data.append("\r") 472 | 473 | for i in range(0,len(data)): 474 | port.write(str(data[i])) 475 | print str(data[i]) 476 | print "packet sent is ", str(data) 477 | 478 | def servo_1_free(): 479 | data = [] 480 | device_id = 4 481 | device_type = 1 482 | function_type = 3 483 | param_count = 0 484 | data.append(chr(device_id)) 485 | data.append(chr(device_type)) 486 | data.append(chr(function_type)) 487 | data.append(chr(param_count)) 488 | data.append("\n") 489 | data.append("\r") 490 | 491 | for i in range(0,len(data)): 492 | port.write(str(data[i])) 493 | print str(data[i]) 494 | print "packet sent is ", str(data) 495 | 496 | def servo_2_free(): 497 | data = [] 498 | device_id = 4 499 | device_type = 1 500 | function_type = 4 501 | param_count = 0 502 | data.append(chr(device_id)) 503 | data.append(chr(device_type)) 504 | data.append(chr(function_type)) 505 | data.append(chr(param_count)) 506 | data.append("\n") 507 | data.append("\r") 508 | 509 | for i in range(0,len(data)): 510 | port.write(str(data[i])) 511 | print str(data[i]) 512 | print "packet sent is ", str(data) 513 | 514 | def servo_3_free(): 515 | data = [] 516 | device_id = 4 517 | device_type = 1 518 | function_type = 5 519 | param_count = 0 520 | data.append(chr(device_id)) 521 | data.append(chr(device_type)) 522 | data.append(chr(function_type)) 523 | data.append(chr(param_count)) 524 | data.append("\n") 525 | data.append("\r") 526 | 527 | for i in range(0,len(data)): 528 | port.write(str(data[i])) 529 | print str(data[i]) 530 | print "packet sent is ", str(data) 531 | #**********************Servo Control Ends********************* 532 | 533 | def led_bargraph_on(led_no): 534 | data = [] 535 | device_id = 5 536 | device_type = 1 537 | function_type = 0 538 | param_count = 1 539 | param_1 = led_no 540 | data.append(chr(device_id)) 541 | data.append(chr(device_type)) 542 | data.append(chr(function_type)) 543 | data.append(chr(param_count)) 544 | data.append(chr(param_1)) 545 | data.append("\n") 546 | data.append("\r") 547 | 548 | for i in range(0,len(data)): 549 | port.write(str(data[i])) 550 | print str(data[i]) 551 | print "packet sent is ", str(data) 552 | 553 | def led_bargraph_off(led_no): 554 | data = [] 555 | device_id = 5 556 | device_type = 1 557 | function_type = 1 558 | param_count = 1 559 | param_1 = (~(led_no)&0xFF) # python ~ is -x-1 two's complement 560 | data.append(chr(device_id)) 561 | data.append(chr(device_type)) 562 | data.append(chr(function_type)) 563 | data.append(chr(param_count)) 564 | data.append(chr(param_1)) 565 | data.append("\n") 566 | data.append("\r") 567 | 568 | for i in range(0,len(data)): 569 | port.write(str(data[i])) 570 | print str(data[i]) 571 | print "packet sent is ", str(data) 572 | 573 | 574 | -------------------------------------------------------------------------------- /Surrogates/Firebird-Code/Firebird_Rpi/Firebird_Rpi.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Firebird_Rpi.c 3 | * 4 | * Created: 23-04-2015 12:21:18 5 | * Author: Saurav Shandilya (e-Yantra Team) 6 | * Application: Development of Raspberry Pi controlled Firebird-V Robot. 7 | * Hardware: Firebird-V Atmega2560 Robot 8 | * 9 | */ 10 | 11 | //**********************Definition and includes********************** 12 | #define F_CPU 14745600 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | //******************************************************************* 19 | 20 | //************************************Variable Declaration Starts***************************** 21 | 22 | unsigned char rec_data = 0; // single byte received at UDR2 is stored in this variable 23 | unsigned char uart_data_buff[25] = {0}; // storing uart data in this buffer 24 | unsigned char copy_packet_data[25] = {0}; // storing uart data into another packet data for operation 25 | unsigned char state = ' '; // for switch cases in UART ISR 26 | unsigned char end_char_rec = 0; 27 | unsigned char i = 0 , j = 0; // 28 | unsigned char data_packet_received = 0; // flag to check if all data_packet is received- goes high when '\!' is received 29 | unsigned char data_copied = 0; // flag to check if uart_data_buff is copied into packet_data 30 | 31 | unsigned char device_id = 0; 32 | unsigned char device_type = 0; 33 | unsigned char function_type = 0; 34 | unsigned char param_count = 0; 35 | unsigned char param_count_upper_nibbel; 36 | unsigned char param_count_lower_nibbel; 37 | unsigned char temp_1; //for data > 8 bit 38 | unsigned char temp_2; //for data > 8 bit 39 | unsigned int param_1 = 0, param_2 = 0, param_3 = 0; 40 | 41 | volatile unsigned long int ShaftCountLeft = 0; //to keep track of left position encoder 42 | volatile unsigned long int ShaftCountRight = 0; //to keep track of right position encoder 43 | volatile unsigned int Degrees; //to accept angle in degrees for turning 44 | 45 | unsigned char data; //to store received data from UDR1 46 | unsigned char ADC_flag; 47 | unsigned char left_motor_velocity = 0x00; 48 | unsigned char right_motor_velocity = 0x00; 49 | 50 | //************************************Variable Declaration Ends***************************** 51 | 52 | 53 | //*******************************Pin Configuration Starts**************************** 54 | 55 | //**********************Buzzer******************************* 56 | void buzzer_pin_config (void) 57 | { 58 | DDRC = DDRC | 0x08; //Setting PORTC 3 as outpt 59 | PORTC = PORTC & 0xF7; //Setting PORTC 3 logic low to turnoff buzzer 60 | } 61 | 62 | //**********************DC Motor******************************* 63 | void motion_pin_config (void) 64 | { 65 | DDRA = DDRA | 0x0F; 66 | PORTA = PORTA & 0xF0; 67 | DDRL = DDRL | 0x18; //Setting PL3 and PL4 pins as output for PWM generation 68 | PORTL = PORTL | 0x18; //PL3 and PL4 pins are for velocity control using PWM. 69 | } 70 | 71 | //**********************Encoder Left and Right******************************* 72 | //Function to configure INT4 (PORTE 4) pin as input for the left position encoder 73 | void left_encoder_pin_config (void) 74 | { 75 | DDRE = DDRE & 0xEF; //Set the direction of the PORTE 4 pin as input 76 | PORTE = PORTE | 0x10; //Enable internal pull-up for PORTE 4 pin 77 | } 78 | 79 | //Function to configure INT5 (PORTE 5) pin as input for the right position encoder 80 | void right_encoder_pin_config (void) 81 | { 82 | DDRE = DDRE & 0xDF; //Set the direction of the PORTE 4 pin as input 83 | PORTE = PORTE | 0x20; //Enable internal pull-up for PORTE 4 pin 84 | } 85 | 86 | //**********************ADC******************************* 87 | void adc_pin_config (void) 88 | { 89 | DDRF = 0x00; //set PORTF direction as input 90 | PORTF = 0x00; //set PORTF pins floating 91 | DDRK = 0x00; //set PORTK direction as input 92 | PORTK = 0x00; //set PORTK pins floating 93 | } 94 | 95 | //**********************Servo motors******************************* 96 | void servo1_pin_config (void) 97 | { 98 | DDRB = DDRB | 0x20; //making PORTB 5 pin output 99 | PORTB = PORTB | 0x20; //setting PORTB 5 pin to logic 1 100 | } 101 | 102 | //Configure PORTB 6 pin for servo motor 2 operation 103 | void servo2_pin_config (void) 104 | { 105 | DDRB = DDRB | 0x40; //making PORTB 6 pin output 106 | PORTB = PORTB | 0x40; //setting PORTB 6 pin to logic 1 107 | } 108 | 109 | //Configure PORTB 7 pin for servo motor 3 operation 110 | void servo3_pin_config (void) 111 | { 112 | DDRB = DDRB | 0x80; //making PORTB 7 pin output 113 | PORTB = PORTB | 0x80; //setting PORTB 7 pin to logic 1 114 | } 115 | 116 | //**********************BarLEDs******************************* 117 | void LED_bargraph_config (void) 118 | { 119 | DDRJ = 0xFF; //PORT J is configured as output 120 | PORTJ = 0x00; //Output is set to 0 121 | } 122 | 123 | //**********************SPI(for sensors on ATmega8)******************************* 124 | void spi_pin_config (void) 125 | { 126 | DDRB = DDRB | 0x07; 127 | PORTB = PORTB | 0x07; 128 | } 129 | 130 | //**********************Port Initilizations******************************* 131 | void port_init() 132 | { 133 | motion_pin_config(); 134 | buzzer_pin_config(); 135 | left_encoder_pin_config(); 136 | right_encoder_pin_config(); 137 | adc_pin_config(); 138 | servo1_pin_config(); 139 | servo2_pin_config(); 140 | servo3_pin_config(); 141 | LED_bargraph_config(); 142 | spi_pin_config(); 143 | } 144 | 145 | //*******************************Pin Configuration Ends**************************** 146 | 147 | //*******************************UART Initialization Starts**************************** 148 | 149 | //Function To Initialize UART2 - Rub robot using USB cable 150 | // desired baud rate:9600 151 | // actual baud rate:9600 (error 0.0%) 152 | // char size: 8 bit 153 | // parity: Disabled 154 | void uart2_init(void) 155 | { 156 | UCSR2B = 0x00; //disable while setting baud rate 157 | UCSR2A = 0x00; 158 | UCSR2C = 0x06; 159 | UBRR2L = 0x5F; //set baud rate lo 160 | UBRR2H = 0x00; //set baud rate hi 161 | UCSR2B = 0x98; 162 | } 163 | //*******************************UART Initialization Ends**************************** 164 | 165 | 166 | //*******************************ADC Initialization Starts**************************** 167 | // Conversion time: 56uS 168 | void adc_init(void) 169 | { 170 | ADCSRA = 0x00; 171 | ADCSRB = 0x00; //MUX5 = 0 172 | ADMUX = 0x20; //Vref=5V external --- ADLAR=1 --- MUX4:0 = 0000 173 | ACSR = 0x80; 174 | ADCSRA = 0x86; //ADEN=1 --- ADIE=1 --- ADPS2:0 = 1 1 0 175 | } 176 | 177 | //*******************************ADC Initialization Ends**************************** 178 | 179 | //*******************************PWM Initialization Starts**************************** 180 | 181 | // Timer 5 initialized in PWM mode for velocity control 182 | // Prescale:256 183 | // PWM 8bit fast, TOP=0x00FF 184 | // Timer Frequency:225.000Hz 185 | void timer5_init() 186 | { 187 | TCCR5B = 0x00; //Stop 188 | TCNT5H = 0xFF; //Counter higher 8-bit value to which OCR5xH value is compared with 189 | TCNT5L = 0x01; //Counter lower 8-bit value to which OCR5xH value is compared with 190 | OCR5AH = 0x00; //Output compare register high value for Left Motor 191 | OCR5AL = 0xFF; //Output compare register low value for Left Motor 192 | OCR5BH = 0x00; //Output compare register high value for Right Motor 193 | OCR5BL = 0xFF; //Output compare register low value for Right Motor 194 | OCR5CH = 0x00; //Output compare register high value for Motor C1 195 | OCR5CL = 0xFF; //Output compare register low value for Motor C1 196 | TCCR5A = 0xA9; /*{COM5A1=1, COM5A0=0; COM5B1=1, COM5B0=0; COM5C1=1 COM5C0=0} 197 | For Overriding normal port functionality to OCRnA outputs. 198 | {WGM51=0, WGM50=1} Along With WGM52 in TCCR5B for Selecting FAST PWM 8-bit Mode*/ 199 | 200 | TCCR5B = 0x0B; //WGM12=1; CS12=0, CS11=1, CS10=1 (Prescaler=64) 201 | } 202 | 203 | //*******************************PWM Initialization Starts**************************** 204 | 205 | //*******************************Timers for Servo Initialization Starts**************************** 206 | 207 | //TIMER1 initialization in 10 bit fast PWM mode 208 | //prescale:256 209 | // WGM: 7) PWM 10bit fast, TOP=0x03FF 210 | // actual value: 52.25Hz 211 | void timer1_init(void) 212 | { 213 | TCCR1B = 0x00; //stop 214 | TCNT1H = 0xFC; //Counter high value to which OCR1xH value is to be compared with 215 | TCNT1L = 0x01; //Counter low value to which OCR1xH value is to be compared with 216 | OCR1AH = 0x03; //Output compare Register high value for servo 1 217 | OCR1AL = 0xFF; //Output Compare Register low Value For servo 1 218 | OCR1BH = 0x03; //Output compare Register high value for servo 2 219 | OCR1BL = 0xFF; //Output Compare Register low Value For servo 2 220 | OCR1CH = 0x03; //Output compare Register high value for servo 3 221 | OCR1CL = 0xFF; //Output Compare Register low Value For servo 3 222 | ICR1H = 0x03; 223 | ICR1L = 0xFF; 224 | TCCR1A = 0xAB; /*{COM1A1=1, COM1A0=0; COM1B1=1, COM1B0=0; COM1C1=1 COM1C0=0} 225 | For Overriding normal port functionality to OCRnA outputs. 226 | {WGM11=1, WGM10=1} Along With WGM12 in TCCR1B for Selecting FAST PWM Mode*/ 227 | TCCR1C = 0x00; 228 | TCCR1B = 0x0C; //WGM12=1; CS12=1, CS11=0, CS10=0 (Prescaler=256) 229 | } 230 | 231 | //*******************************Timer for Servo Initialization Ends**************************** 232 | 233 | //*******************************Interrupt for position encoder Initialization Starts**************************** 234 | 235 | void left_position_encoder_interrupt_init (void) //Interrupt 4 enable 236 | { 237 | cli(); //Clears the global interrupt 238 | EICRB = EICRB | 0x02; // INT4 is set to trigger with falling edge 239 | EIMSK = EIMSK | 0x10; // Enable Interrupt INT4 for left position encoder 240 | sei(); // Enables the global interrupt 241 | } 242 | 243 | void right_position_encoder_interrupt_init (void) //Interrupt 5 enable 244 | { 245 | cli(); //Clears the global interrupt 246 | EICRB = EICRB | 0x08; // INT5 is set to trigger with falling edge 247 | EIMSK = EIMSK | 0x20; // Enable Interrupt INT5 for right position encoder 248 | sei(); // Enables the global interrupt 249 | } 250 | 251 | //*******************************Interrupt for position encoder Initialization ends**************************** 252 | 253 | //*******************************SPI Initialization Starts**************************** 254 | //SPI initialize 255 | // clock rate: 921600hz 256 | void spi_init(void) 257 | { 258 | SPCR = 0x53; //setup SPI 259 | SPSR = 0x00; //setup SPI 260 | SPDR = 0x00; 261 | } 262 | //*******************************SPI Initialization Ends**************************** 263 | 264 | //ISR for right position encoder 265 | ISR(INT5_vect) 266 | { 267 | ShaftCountRight++; //increment right shaft position count 268 | } 269 | 270 | 271 | //ISR for left position encoder 272 | ISR(INT4_vect) 273 | { 274 | ShaftCountLeft++; //increment left shaft position count 275 | } 276 | 277 | //Function To Initialize all The Devices 278 | void init_devices() 279 | { 280 | cli(); //Clears the global interrupts 281 | port_init(); //Initializes all the ports 282 | uart2_init(); //Initialize UART1 for serial communication 283 | adc_init(); 284 | timer5_init(); // timer for PWM generation 285 | left_position_encoder_interrupt_init(); 286 | right_position_encoder_interrupt_init(); 287 | timer1_init(); // timer for servo motors 288 | spi_init(); 289 | sei(); //Enables the global interrupts 290 | } 291 | 292 | //------------------------------------------------------------------------------- 293 | //-- ADC Conversion Function -------------- 294 | //------------------------------------------------------------------------------- 295 | unsigned char ADC_Conversion(unsigned char ch) 296 | { 297 | unsigned char a; 298 | if(ch>7) 299 | { 300 | ADCSRB = 0x08; 301 | } 302 | ch = ch & 0x07; //Store only 3 LSB bits 303 | ADMUX= 0x20 | ch; //Select the ADC channel with left adjust select 304 | ADC_flag = 0x00; //Clear the user defined flag 305 | ADCSRA = ADCSRA | 0x40; //Set start conversion bit 306 | while((ADCSRA&0x10)==0); //Wait for ADC conversion to complete 307 | a=ADCH; 308 | ADCSRA = ADCSRA|0x10; //clear ADIF (ADC Interrupt Flag) by writing 1 to it 309 | ADCSRB = 0x00; 310 | return a; 311 | } 312 | 313 | // Function for robot velocity control 314 | void velocity (unsigned char left_motor, unsigned char right_motor) 315 | { 316 | OCR5AL = (unsigned char)left_motor; 317 | OCR5BL = (unsigned char)right_motor; 318 | } 319 | 320 | void motor_enable (void) 321 | { 322 | PORTL |= 18; // Enable left and right motor. Used with function where velocity is not used 323 | } 324 | void buzzer_on (void) 325 | { 326 | PORTC |= 0x08; 327 | } 328 | 329 | void buzzer_off (void) 330 | { 331 | PORTC &= 0xF7; 332 | } 333 | 334 | void forward (void) 335 | { 336 | //PORTA &= 0xF0; 337 | PORTA = 0x06; 338 | } 339 | 340 | void back (void) 341 | { 342 | //PORTA &= 0xF0; 343 | PORTA = 0x09; 344 | } 345 | 346 | void left (void) 347 | { 348 | //PORTA &= 0xF0; 349 | PORTA = 0x05; 350 | } 351 | 352 | void right (void) 353 | { 354 | //PORTA &= 0xF0; 355 | PORTA = 0x0A; 356 | } 357 | 358 | void stop (void) 359 | { 360 | PORTA = 0x00; 361 | } 362 | 363 | //Function used for moving robot forward by specified distance 364 | 365 | void linear_distance_mm(unsigned int DistanceInMM) 366 | { 367 | float ReqdShaftCount = 0; 368 | unsigned long int ReqdShaftCountInt = 0; 369 | 370 | ReqdShaftCount = DistanceInMM / 5.338; // division by resolution to get shaft count 371 | ReqdShaftCountInt = (unsigned long int) ReqdShaftCount; 372 | 373 | ShaftCountRight = 0; 374 | while(1) 375 | { 376 | if(ShaftCountRight > ReqdShaftCountInt) 377 | { 378 | break; 379 | } 380 | } 381 | stop(); //Stop robot 382 | } 383 | 384 | void forward_mm(unsigned int DistanceInMM) 385 | { 386 | forward(); 387 | linear_distance_mm(DistanceInMM); 388 | } 389 | 390 | void back_mm(unsigned int DistanceInMM) 391 | { 392 | back(); 393 | linear_distance_mm(DistanceInMM); 394 | } 395 | 396 | //Function to rotate Servo 1 by a specified angle in the multiples of 1.86 degrees 397 | void servo_1(unsigned char degrees) 398 | { 399 | float PositionPanServo = 0; 400 | PositionPanServo = ((float)degrees / 1.86) + 35.0; 401 | OCR1AH = 0x00; 402 | OCR1AL = (unsigned char) PositionPanServo; 403 | } 404 | 405 | 406 | //Function to rotate Servo 2 by a specified angle in the multiples of 1.86 degrees 407 | void servo_2(unsigned char degrees) 408 | { 409 | float PositionTiltServo = 0; 410 | PositionTiltServo = ((float)degrees / 1.86) + 35.0; 411 | OCR1BH = 0x00; 412 | OCR1BL = (unsigned char) PositionTiltServo; 413 | } 414 | 415 | //Function to rotate Servo 3 by a specified angle in the multiples of 1.86 degrees 416 | void servo_3(unsigned char degrees) 417 | { 418 | float PositionServo = 0; 419 | PositionServo = ((float)degrees / 1.86) + 35.0; 420 | OCR1CH = 0x00; 421 | OCR1CL = (unsigned char) PositionServo; 422 | } 423 | 424 | //servo_free functions unlocks the servo motors from the any angle 425 | //and make them free by giving 100% duty cycle at the PWM. This function can be used to 426 | //reduce the power consumption of the motor if it is holding load against the gravity. 427 | 428 | void servo_1_free (void) //makes servo 1 free rotating 429 | { 430 | OCR1AH = 0x03; 431 | OCR1AL = 0xFF; //Servo 1 off 432 | } 433 | 434 | void servo_2_free (void) //makes servo 2 free rotating 435 | { 436 | OCR1BH = 0x03; 437 | OCR1BL = 0xFF; //Servo 2 off 438 | } 439 | 440 | void servo_3_free (void) //makes servo 3 free rotating 441 | { 442 | OCR1CH = 0x03; 443 | OCR1CL = 0xFF; //Servo 3 off 444 | } 445 | 446 | void LED_bargraph_on (unsigned char data) 447 | { 448 | PORTJ |= data; 449 | } 450 | 451 | void LED_bargraph_off (unsigned char data) 452 | { 453 | PORTJ &= data; 454 | } 455 | 456 | //------------------------------------------------------------------------------- 457 | //-- Function To Transmit/Receive through SPI interface ------ 458 | //------------------------------------------------------------------------------- 459 | unsigned char spi_master_tx_and_rx (unsigned char data) 460 | { 461 | unsigned char rx_data = 0; 462 | 463 | PORTB = PORTB & 0xFE; // make SS pin low 464 | SPDR = data; 465 | while(!(SPSR & (1<: 34 | 0: 0c 94 72 00 jmp 0xe4 ; 0xe4 <__ctors_end> 35 | 4: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 36 | 8: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 37 | c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 38 | 10: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 39 | 14: 0c 94 78 01 jmp 0x2f0 ; 0x2f0 <__vector_5> 40 | 18: 0c 94 53 01 jmp 0x2a6 ; 0x2a6 <__vector_6> 41 | 1c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 42 | 20: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 43 | 24: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 44 | 28: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 45 | 2c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 46 | 30: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 47 | 34: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 48 | 38: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 49 | 3c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 50 | 40: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 51 | 44: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 52 | 48: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 53 | 4c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 54 | 50: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 55 | 54: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 56 | 58: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 57 | 5c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 58 | 60: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 59 | 64: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 60 | 68: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 61 | 6c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 62 | 70: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 63 | 74: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 64 | 78: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 65 | 7c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 66 | 80: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 67 | 84: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 68 | 88: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 69 | 8c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 70 | 90: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 71 | 94: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 72 | 98: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 73 | 9c: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 74 | a0: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 75 | a4: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 76 | a8: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 77 | ac: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 78 | b0: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 79 | b4: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 80 | b8: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 81 | bc: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 82 | c0: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 83 | c4: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 84 | c8: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 85 | cc: 0c 94 a7 02 jmp 0x54e ; 0x54e <__vector_51> 86 | d0: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 87 | d4: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 88 | d8: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 89 | dc: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 90 | e0: 0c 94 93 00 jmp 0x126 ; 0x126 <__bad_interrupt> 91 | 92 | 000000e4 <__ctors_end>: 93 | e4: 11 24 eor r1, r1 94 | e6: 1f be out 0x3f, r1 ; 63 95 | e8: cf ef ldi r28, 0xFF ; 255 96 | ea: d1 e2 ldi r29, 0x21 ; 33 97 | ec: de bf out 0x3e, r29 ; 62 98 | ee: cd bf out 0x3d, r28 ; 61 99 | f0: 00 e0 ldi r16, 0x00 ; 0 100 | f2: 0c bf out 0x3c, r16 ; 60 101 | 102 | 000000f4 <__do_copy_data>: 103 | f4: 12 e0 ldi r17, 0x02 ; 2 104 | f6: a0 e0 ldi r26, 0x00 ; 0 105 | f8: b2 e0 ldi r27, 0x02 ; 2 106 | fa: e2 ec ldi r30, 0xC2 ; 194 107 | fc: fc e0 ldi r31, 0x0C ; 12 108 | fe: 00 e0 ldi r16, 0x00 ; 0 109 | 100: 0b bf out 0x3b, r16 ; 59 110 | 102: 02 c0 rjmp .+4 ; 0x108 <__do_copy_data+0x14> 111 | 104: 07 90 elpm r0, Z+ 112 | 106: 0d 92 st X+, r0 113 | 108: a2 30 cpi r26, 0x02 ; 2 114 | 10a: b1 07 cpc r27, r17 115 | 10c: d9 f7 brne .-10 ; 0x104 <__do_copy_data+0x10> 116 | 117 | 0000010e <__do_clear_bss>: 118 | 10e: 12 e0 ldi r17, 0x02 ; 2 119 | 110: a2 e0 ldi r26, 0x02 ; 2 120 | 112: b2 e0 ldi r27, 0x02 ; 2 121 | 114: 01 c0 rjmp .+2 ; 0x118 <.do_clear_bss_start> 122 | 123 | 00000116 <.do_clear_bss_loop>: 124 | 116: 1d 92 st X+, r1 125 | 126 | 00000118 <.do_clear_bss_start>: 127 | 118: a6 35 cpi r26, 0x56 ; 86 128 | 11a: b1 07 cpc r27, r17 129 | 11c: e1 f7 brne .-8 ; 0x116 <.do_clear_bss_loop> 130 | 11e: 0e 94 cd 04 call 0x99a ; 0x99a
131 | 122: 0c 94 5f 06 jmp 0xcbe ; 0xcbe <_exit> 132 | 133 | 00000126 <__bad_interrupt>: 134 | 126: 0c 94 00 00 jmp 0 ; 0x0 <__vectors> 135 | 136 | 0000012a : 137 | //*******************************Pin Configuration Starts**************************** 138 | 139 | //**********************Buzzer******************************* 140 | void buzzer_pin_config (void) 141 | { 142 | DDRC = DDRC | 0x08; //Setting PORTC 3 as outpt 143 | 12a: 3b 9a sbi 0x07, 3 ; 7 144 | PORTC = PORTC & 0xF7; //Setting PORTC 3 logic low to turnoff buzzer 145 | 12c: 43 98 cbi 0x08, 3 ; 8 146 | } 147 | 12e: 08 95 ret 148 | 149 | 00000130 : 150 | 151 | //**********************DC Motor******************************* 152 | void motion_pin_config (void) 153 | { 154 | DDRA = DDRA | 0x0F; 155 | 130: 81 b1 in r24, 0x01 ; 1 156 | 132: 8f 60 ori r24, 0x0F ; 15 157 | 134: 81 b9 out 0x01, r24 ; 1 158 | PORTA = PORTA & 0xF0; 159 | 136: 82 b1 in r24, 0x02 ; 2 160 | 138: 80 7f andi r24, 0xF0 ; 240 161 | 13a: 82 b9 out 0x02, r24 ; 2 162 | DDRL = DDRL | 0x18; //Setting PL3 and PL4 pins as output for PWM generation 163 | 13c: ea e0 ldi r30, 0x0A ; 10 164 | 13e: f1 e0 ldi r31, 0x01 ; 1 165 | 140: 80 81 ld r24, Z 166 | 142: 88 61 ori r24, 0x18 ; 24 167 | 144: 80 83 st Z, r24 168 | PORTL = PORTL | 0x18; //PL3 and PL4 pins are for velocity control using PWM. 169 | 146: eb e0 ldi r30, 0x0B ; 11 170 | 148: f1 e0 ldi r31, 0x01 ; 1 171 | 14a: 80 81 ld r24, Z 172 | 14c: 88 61 ori r24, 0x18 ; 24 173 | 14e: 80 83 st Z, r24 174 | } 175 | 150: 08 95 ret 176 | 177 | 00000152 : 178 | 179 | //**********************Encoder Left and Right******************************* 180 | //Function to configure INT4 (PORTE 4) pin as input for the left position encoder 181 | void left_encoder_pin_config (void) 182 | { 183 | DDRE = DDRE & 0xEF; //Set the direction of the PORTE 4 pin as input 184 | 152: 6c 98 cbi 0x0d, 4 ; 13 185 | PORTE = PORTE | 0x10; //Enable internal pull-up for PORTE 4 pin 186 | 154: 74 9a sbi 0x0e, 4 ; 14 187 | } 188 | 156: 08 95 ret 189 | 190 | 00000158 : 191 | 192 | //Function to configure INT5 (PORTE 5) pin as input for the right position encoder 193 | void right_encoder_pin_config (void) 194 | { 195 | DDRE = DDRE & 0xDF; //Set the direction of the PORTE 4 pin as input 196 | 158: 6d 98 cbi 0x0d, 5 ; 13 197 | PORTE = PORTE | 0x20; //Enable internal pull-up for PORTE 4 pin 198 | 15a: 75 9a sbi 0x0e, 5 ; 14 199 | } 200 | 15c: 08 95 ret 201 | 202 | 0000015e : 203 | 204 | //**********************ADC******************************* 205 | void adc_pin_config (void) 206 | { 207 | DDRF = 0x00; //set PORTF direction as input 208 | 15e: 10 ba out 0x10, r1 ; 16 209 | PORTF = 0x00; //set PORTF pins floating 210 | 160: 11 ba out 0x11, r1 ; 17 211 | DDRK = 0x00; //set PORTK direction as input 212 | 162: 10 92 07 01 sts 0x0107, r1 213 | PORTK = 0x00; //set PORTK pins floating 214 | 166: 10 92 08 01 sts 0x0108, r1 215 | } 216 | 16a: 08 95 ret 217 | 218 | 0000016c : 219 | 220 | //**********************Servo motors******************************* 221 | void servo1_pin_config (void) 222 | { 223 | DDRB = DDRB | 0x20; //making PORTB 5 pin output 224 | 16c: 25 9a sbi 0x04, 5 ; 4 225 | PORTB = PORTB | 0x20; //setting PORTB 5 pin to logic 1 226 | 16e: 2d 9a sbi 0x05, 5 ; 5 227 | } 228 | 170: 08 95 ret 229 | 230 | 00000172 : 231 | 232 | //Configure PORTB 6 pin for servo motor 2 operation 233 | void servo2_pin_config (void) 234 | { 235 | DDRB = DDRB | 0x40; //making PORTB 6 pin output 236 | 172: 26 9a sbi 0x04, 6 ; 4 237 | PORTB = PORTB | 0x40; //setting PORTB 6 pin to logic 1 238 | 174: 2e 9a sbi 0x05, 6 ; 5 239 | } 240 | 176: 08 95 ret 241 | 242 | 00000178 : 243 | 244 | //Configure PORTB 7 pin for servo motor 3 operation 245 | void servo3_pin_config (void) 246 | { 247 | DDRB = DDRB | 0x80; //making PORTB 7 pin output 248 | 178: 27 9a sbi 0x04, 7 ; 4 249 | PORTB = PORTB | 0x80; //setting PORTB 7 pin to logic 1 250 | 17a: 2f 9a sbi 0x05, 7 ; 5 251 | } 252 | 17c: 08 95 ret 253 | 254 | 0000017e : 255 | 256 | //**********************BarLEDs******************************* 257 | void LED_bargraph_config (void) 258 | { 259 | DDRJ = 0xFF; //PORT J is configured as output 260 | 17e: 8f ef ldi r24, 0xFF ; 255 261 | 180: 80 93 04 01 sts 0x0104, r24 262 | PORTJ = 0x00; //Output is set to 0 263 | 184: 10 92 05 01 sts 0x0105, r1 264 | } 265 | 188: 08 95 ret 266 | 267 | 0000018a : 268 | 269 | //**********************SPI(for sensors on ATmega8)******************************* 270 | void spi_pin_config (void) 271 | { 272 | DDRB = DDRB | 0x07; 273 | 18a: 84 b1 in r24, 0x04 ; 4 274 | 18c: 87 60 ori r24, 0x07 ; 7 275 | 18e: 84 b9 out 0x04, r24 ; 4 276 | PORTB = PORTB | 0x07; 277 | 190: 85 b1 in r24, 0x05 ; 5 278 | 192: 87 60 ori r24, 0x07 ; 7 279 | 194: 85 b9 out 0x05, r24 ; 5 280 | } 281 | 196: 08 95 ret 282 | 283 | 00000198 : 284 | 285 | //**********************Port Initilizations******************************* 286 | void port_init() 287 | { 288 | motion_pin_config(); 289 | 198: 0e 94 98 00 call 0x130 ; 0x130 290 | buzzer_pin_config(); 291 | 19c: 0e 94 95 00 call 0x12a ; 0x12a 292 | left_encoder_pin_config(); 293 | 1a0: 0e 94 a9 00 call 0x152 ; 0x152 294 | right_encoder_pin_config(); 295 | 1a4: 0e 94 ac 00 call 0x158 ; 0x158 296 | adc_pin_config(); 297 | 1a8: 0e 94 af 00 call 0x15e ; 0x15e 298 | servo1_pin_config(); 299 | 1ac: 0e 94 b6 00 call 0x16c ; 0x16c 300 | servo2_pin_config(); 301 | 1b0: 0e 94 b9 00 call 0x172 ; 0x172 302 | servo3_pin_config(); 303 | 1b4: 0e 94 bc 00 call 0x178 ; 0x178 304 | LED_bargraph_config(); 305 | 1b8: 0e 94 bf 00 call 0x17e ; 0x17e 306 | spi_pin_config(); 307 | 1bc: 0e 94 c5 00 call 0x18a ; 0x18a 308 | } 309 | 1c0: 08 95 ret 310 | 311 | 000001c2 : 312 | // actual baud rate:9600 (error 0.0%) 313 | // char size: 8 bit 314 | // parity: Disabled 315 | void uart2_init(void) 316 | { 317 | UCSR2B = 0x00; //disable while setting baud rate 318 | 1c2: e1 ed ldi r30, 0xD1 ; 209 319 | 1c4: f0 e0 ldi r31, 0x00 ; 0 320 | 1c6: 10 82 st Z, r1 321 | UCSR2A = 0x00; 322 | 1c8: 10 92 d0 00 sts 0x00D0, r1 323 | UCSR2C = 0x06; 324 | 1cc: 86 e0 ldi r24, 0x06 ; 6 325 | 1ce: 80 93 d2 00 sts 0x00D2, r24 326 | UBRR2L = 0x5F; //set baud rate lo 327 | 1d2: 8f e5 ldi r24, 0x5F ; 95 328 | 1d4: 80 93 d4 00 sts 0x00D4, r24 329 | UBRR2H = 0x00; //set baud rate hi 330 | 1d8: 10 92 d5 00 sts 0x00D5, r1 331 | UCSR2B = 0x98; 332 | 1dc: 88 e9 ldi r24, 0x98 ; 152 333 | 1de: 80 83 st Z, r24 334 | } 335 | 1e0: 08 95 ret 336 | 337 | 000001e2 : 338 | 339 | //*******************************ADC Initialization Starts**************************** 340 | // Conversion time: 56uS 341 | void adc_init(void) 342 | { 343 | ADCSRA = 0x00; 344 | 1e2: ea e7 ldi r30, 0x7A ; 122 345 | 1e4: f0 e0 ldi r31, 0x00 ; 0 346 | 1e6: 10 82 st Z, r1 347 | ADCSRB = 0x00; //MUX5 = 0 348 | 1e8: 10 92 7b 00 sts 0x007B, r1 349 | ADMUX = 0x20; //Vref=5V external --- ADLAR=1 --- MUX4:0 = 0000 350 | 1ec: 80 e2 ldi r24, 0x20 ; 32 351 | 1ee: 80 93 7c 00 sts 0x007C, r24 352 | ACSR = 0x80; 353 | 1f2: 80 e8 ldi r24, 0x80 ; 128 354 | 1f4: 80 bf out 0x30, r24 ; 48 355 | ADCSRA = 0x86; //ADEN=1 --- ADIE=1 --- ADPS2:0 = 1 1 0 356 | 1f6: 86 e8 ldi r24, 0x86 ; 134 357 | 1f8: 80 83 st Z, r24 358 | } 359 | 1fa: 08 95 ret 360 | 361 | 000001fc : 362 | // Prescale:256 363 | // PWM 8bit fast, TOP=0x00FF 364 | // Timer Frequency:225.000Hz 365 | void timer5_init() 366 | { 367 | TCCR5B = 0x00; //Stop 368 | 1fc: e1 e2 ldi r30, 0x21 ; 33 369 | 1fe: f1 e0 ldi r31, 0x01 ; 1 370 | 200: 10 82 st Z, r1 371 | TCNT5H = 0xFF; //Counter higher 8-bit value to which OCR5xH value is compared with 372 | 202: 8f ef ldi r24, 0xFF ; 255 373 | 204: 80 93 25 01 sts 0x0125, r24 374 | TCNT5L = 0x01; //Counter lower 8-bit value to which OCR5xH value is compared with 375 | 208: 91 e0 ldi r25, 0x01 ; 1 376 | 20a: 90 93 24 01 sts 0x0124, r25 377 | OCR5AH = 0x00; //Output compare register high value for Left Motor 378 | 20e: 10 92 29 01 sts 0x0129, r1 379 | OCR5AL = 0xFF; //Output compare register low value for Left Motor 380 | 212: 80 93 28 01 sts 0x0128, r24 381 | OCR5BH = 0x00; //Output compare register high value for Right Motor 382 | 216: 10 92 2b 01 sts 0x012B, r1 383 | OCR5BL = 0xFF; //Output compare register low value for Right Motor 384 | 21a: 80 93 2a 01 sts 0x012A, r24 385 | OCR5CH = 0x00; //Output compare register high value for Motor C1 386 | 21e: 10 92 2d 01 sts 0x012D, r1 387 | OCR5CL = 0xFF; //Output compare register low value for Motor C1 388 | 222: 80 93 2c 01 sts 0x012C, r24 389 | TCCR5A = 0xA9; /*{COM5A1=1, COM5A0=0; COM5B1=1, COM5B0=0; COM5C1=1 COM5C0=0} 390 | 226: 89 ea ldi r24, 0xA9 ; 169 391 | 228: 80 93 20 01 sts 0x0120, r24 392 | For Overriding normal port functionality to OCRnA outputs. 393 | {WGM51=0, WGM50=1} Along With WGM52 in TCCR5B for Selecting FAST PWM 8-bit Mode*/ 394 | 395 | TCCR5B = 0x0B; //WGM12=1; CS12=0, CS11=1, CS10=1 (Prescaler=64) 396 | 22c: 8b e0 ldi r24, 0x0B ; 11 397 | 22e: 80 83 st Z, r24 398 | } 399 | 230: 08 95 ret 400 | 401 | 00000232 : 402 | //prescale:256 403 | // WGM: 7) PWM 10bit fast, TOP=0x03FF 404 | // actual value: 52.25Hz 405 | void timer1_init(void) 406 | { 407 | TCCR1B = 0x00; //stop 408 | 232: e1 e8 ldi r30, 0x81 ; 129 409 | 234: f0 e0 ldi r31, 0x00 ; 0 410 | 236: 10 82 st Z, r1 411 | TCNT1H = 0xFC; //Counter high value to which OCR1xH value is to be compared with 412 | 238: 8c ef ldi r24, 0xFC ; 252 413 | 23a: 80 93 85 00 sts 0x0085, r24 414 | TCNT1L = 0x01; //Counter low value to which OCR1xH value is to be compared with 415 | 23e: 81 e0 ldi r24, 0x01 ; 1 416 | 240: 80 93 84 00 sts 0x0084, r24 417 | OCR1AH = 0x03; //Output compare Register high value for servo 1 418 | 244: 93 e0 ldi r25, 0x03 ; 3 419 | 246: 90 93 89 00 sts 0x0089, r25 420 | OCR1AL = 0xFF; //Output Compare Register low Value For servo 1 421 | 24a: 8f ef ldi r24, 0xFF ; 255 422 | 24c: 80 93 88 00 sts 0x0088, r24 423 | OCR1BH = 0x03; //Output compare Register high value for servo 2 424 | 250: 90 93 8b 00 sts 0x008B, r25 425 | OCR1BL = 0xFF; //Output Compare Register low Value For servo 2 426 | 254: 80 93 8a 00 sts 0x008A, r24 427 | OCR1CH = 0x03; //Output compare Register high value for servo 3 428 | 258: 90 93 8d 00 sts 0x008D, r25 429 | OCR1CL = 0xFF; //Output Compare Register low Value For servo 3 430 | 25c: 80 93 8c 00 sts 0x008C, r24 431 | ICR1H = 0x03; 432 | 260: 90 93 87 00 sts 0x0087, r25 433 | ICR1L = 0xFF; 434 | 264: 80 93 86 00 sts 0x0086, r24 435 | TCCR1A = 0xAB; /*{COM1A1=1, COM1A0=0; COM1B1=1, COM1B0=0; COM1C1=1 COM1C0=0} 436 | 268: 8b ea ldi r24, 0xAB ; 171 437 | 26a: 80 93 80 00 sts 0x0080, r24 438 | For Overriding normal port functionality to OCRnA outputs. 439 | {WGM11=1, WGM10=1} Along With WGM12 in TCCR1B for Selecting FAST PWM Mode*/ 440 | TCCR1C = 0x00; 441 | 26e: 10 92 82 00 sts 0x0082, r1 442 | TCCR1B = 0x0C; //WGM12=1; CS12=1, CS11=0, CS10=0 (Prescaler=256) 443 | 272: 8c e0 ldi r24, 0x0C ; 12 444 | 274: 80 83 st Z, r24 445 | } 446 | 276: 08 95 ret 447 | 448 | 00000278 : 449 | 450 | //*******************************Interrupt for position encoder Initialization Starts**************************** 451 | 452 | void left_position_encoder_interrupt_init (void) //Interrupt 4 enable 453 | { 454 | cli(); //Clears the global interrupt 455 | 278: f8 94 cli 456 | EICRB = EICRB | 0x02; // INT4 is set to trigger with falling edge 457 | 27a: ea e6 ldi r30, 0x6A ; 106 458 | 27c: f0 e0 ldi r31, 0x00 ; 0 459 | 27e: 80 81 ld r24, Z 460 | 280: 82 60 ori r24, 0x02 ; 2 461 | 282: 80 83 st Z, r24 462 | EIMSK = EIMSK | 0x10; // Enable Interrupt INT4 for left position encoder 463 | 284: ec 9a sbi 0x1d, 4 ; 29 464 | sei(); // Enables the global interrupt 465 | 286: 78 94 sei 466 | } 467 | 288: 08 95 ret 468 | 469 | 0000028a : 470 | 471 | void right_position_encoder_interrupt_init (void) //Interrupt 5 enable 472 | { 473 | cli(); //Clears the global interrupt 474 | 28a: f8 94 cli 475 | EICRB = EICRB | 0x08; // INT5 is set to trigger with falling edge 476 | 28c: ea e6 ldi r30, 0x6A ; 106 477 | 28e: f0 e0 ldi r31, 0x00 ; 0 478 | 290: 80 81 ld r24, Z 479 | 292: 88 60 ori r24, 0x08 ; 8 480 | 294: 80 83 st Z, r24 481 | EIMSK = EIMSK | 0x20; // Enable Interrupt INT5 for right position encoder 482 | 296: ed 9a sbi 0x1d, 5 ; 29 483 | sei(); // Enables the global interrupt 484 | 298: 78 94 sei 485 | } 486 | 29a: 08 95 ret 487 | 488 | 0000029c : 489 | //*******************************SPI Initialization Starts**************************** 490 | //SPI initialize 491 | // clock rate: 921600hz 492 | void spi_init(void) 493 | { 494 | SPCR = 0x53; //setup SPI 495 | 29c: 83 e5 ldi r24, 0x53 ; 83 496 | 29e: 8c bd out 0x2c, r24 ; 44 497 | SPSR = 0x00; //setup SPI 498 | 2a0: 1d bc out 0x2d, r1 ; 45 499 | SPDR = 0x00; 500 | 2a2: 1e bc out 0x2e, r1 ; 46 501 | } 502 | 2a4: 08 95 ret 503 | 504 | 000002a6 <__vector_6>: 505 | //*******************************SPI Initialization Ends**************************** 506 | 507 | //ISR for right position encoder 508 | ISR(INT5_vect) 509 | { 510 | 2a6: 1f 92 push r1 511 | 2a8: 0f 92 push r0 512 | 2aa: 0f b6 in r0, 0x3f ; 63 513 | 2ac: 0f 92 push r0 514 | 2ae: 11 24 eor r1, r1 515 | 2b0: 8f 93 push r24 516 | 2b2: 9f 93 push r25 517 | 2b4: af 93 push r26 518 | 2b6: bf 93 push r27 519 | ShaftCountRight++; //increment right shaft position count 520 | 2b8: 80 91 04 02 lds r24, 0x0204 521 | 2bc: 90 91 05 02 lds r25, 0x0205 522 | 2c0: a0 91 06 02 lds r26, 0x0206 523 | 2c4: b0 91 07 02 lds r27, 0x0207 524 | 2c8: 01 96 adiw r24, 0x01 ; 1 525 | 2ca: a1 1d adc r26, r1 526 | 2cc: b1 1d adc r27, r1 527 | 2ce: 80 93 04 02 sts 0x0204, r24 528 | 2d2: 90 93 05 02 sts 0x0205, r25 529 | 2d6: a0 93 06 02 sts 0x0206, r26 530 | 2da: b0 93 07 02 sts 0x0207, r27 531 | } 532 | 2de: bf 91 pop r27 533 | 2e0: af 91 pop r26 534 | 2e2: 9f 91 pop r25 535 | 2e4: 8f 91 pop r24 536 | 2e6: 0f 90 pop r0 537 | 2e8: 0f be out 0x3f, r0 ; 63 538 | 2ea: 0f 90 pop r0 539 | 2ec: 1f 90 pop r1 540 | 2ee: 18 95 reti 541 | 542 | 000002f0 <__vector_5>: 543 | 544 | 545 | //ISR for left position encoder 546 | ISR(INT4_vect) 547 | { 548 | 2f0: 1f 92 push r1 549 | 2f2: 0f 92 push r0 550 | 2f4: 0f b6 in r0, 0x3f ; 63 551 | 2f6: 0f 92 push r0 552 | 2f8: 11 24 eor r1, r1 553 | 2fa: 8f 93 push r24 554 | 2fc: 9f 93 push r25 555 | 2fe: af 93 push r26 556 | 300: bf 93 push r27 557 | ShaftCountLeft++; //increment left shaft position count 558 | 302: 80 91 08 02 lds r24, 0x0208 559 | 306: 90 91 09 02 lds r25, 0x0209 560 | 30a: a0 91 0a 02 lds r26, 0x020A 561 | 30e: b0 91 0b 02 lds r27, 0x020B 562 | 312: 01 96 adiw r24, 0x01 ; 1 563 | 314: a1 1d adc r26, r1 564 | 316: b1 1d adc r27, r1 565 | 318: 80 93 08 02 sts 0x0208, r24 566 | 31c: 90 93 09 02 sts 0x0209, r25 567 | 320: a0 93 0a 02 sts 0x020A, r26 568 | 324: b0 93 0b 02 sts 0x020B, r27 569 | } 570 | 328: bf 91 pop r27 571 | 32a: af 91 pop r26 572 | 32c: 9f 91 pop r25 573 | 32e: 8f 91 pop r24 574 | 330: 0f 90 pop r0 575 | 332: 0f be out 0x3f, r0 ; 63 576 | 334: 0f 90 pop r0 577 | 336: 1f 90 pop r1 578 | 338: 18 95 reti 579 | 580 | 0000033a : 581 | 582 | //Function To Initialize all The Devices 583 | void init_devices() 584 | { 585 | cli(); //Clears the global interrupts 586 | 33a: f8 94 cli 587 | port_init(); //Initializes all the ports 588 | 33c: 0e 94 cc 00 call 0x198 ; 0x198 589 | uart2_init(); //Initialize UART1 for serial communication 590 | 340: 0e 94 e1 00 call 0x1c2 ; 0x1c2 591 | adc_init(); 592 | 344: 0e 94 f1 00 call 0x1e2 ; 0x1e2 593 | timer5_init(); // timer for PWM generation 594 | 348: 0e 94 fe 00 call 0x1fc ; 0x1fc 595 | left_position_encoder_interrupt_init(); 596 | 34c: 0e 94 3c 01 call 0x278 ; 0x278 597 | right_position_encoder_interrupt_init(); 598 | 350: 0e 94 45 01 call 0x28a ; 0x28a 599 | timer1_init(); // timer for servo motors 600 | 354: 0e 94 19 01 call 0x232 ; 0x232 601 | spi_init(); 602 | 358: 0e 94 4e 01 call 0x29c ; 0x29c 603 | sei(); //Enables the global interrupts 604 | 35c: 78 94 sei 605 | } 606 | 35e: 08 95 ret 607 | 608 | 00000360 : 609 | //-- ADC Conversion Function -------------- 610 | //------------------------------------------------------------------------------- 611 | unsigned char ADC_Conversion(unsigned char ch) 612 | { 613 | unsigned char a; 614 | if(ch>7) 615 | 360: 88 30 cpi r24, 0x08 ; 8 616 | 362: 18 f0 brcs .+6 ; 0x36a 617 | { 618 | ADCSRB = 0x08; 619 | 364: 98 e0 ldi r25, 0x08 ; 8 620 | 366: 90 93 7b 00 sts 0x007B, r25 621 | } 622 | ch = ch & 0x07; //Store only 3 LSB bits 623 | 36a: 87 70 andi r24, 0x07 ; 7 624 | ADMUX= 0x20 | ch; //Select the ADC channel with left adjust select 625 | 36c: 80 62 ori r24, 0x20 ; 32 626 | 36e: 80 93 7c 00 sts 0x007C, r24 627 | ADC_flag = 0x00; //Clear the user defined flag 628 | 372: 10 92 4e 02 sts 0x024E, r1 629 | ADCSRA = ADCSRA | 0x40; //Set start conversion bit 630 | 376: ea e7 ldi r30, 0x7A ; 122 631 | 378: f0 e0 ldi r31, 0x00 ; 0 632 | 37a: 80 81 ld r24, Z 633 | 37c: 80 64 ori r24, 0x40 ; 64 634 | 37e: 80 83 st Z, r24 635 | while((ADCSRA&0x10)==0); //Wait for ADC conversion to complete 636 | 380: 80 81 ld r24, Z 637 | 382: 84 ff sbrs r24, 4 638 | 384: fd cf rjmp .-6 ; 0x380 639 | a=ADCH; 640 | 386: 80 91 79 00 lds r24, 0x0079 641 | ADCSRA = ADCSRA|0x10; //clear ADIF (ADC Interrupt Flag) by writing 1 to it 642 | 38a: ea e7 ldi r30, 0x7A ; 122 643 | 38c: f0 e0 ldi r31, 0x00 ; 0 644 | 38e: 90 81 ld r25, Z 645 | 390: 90 61 ori r25, 0x10 ; 16 646 | 392: 90 83 st Z, r25 647 | ADCSRB = 0x00; 648 | 394: 10 92 7b 00 sts 0x007B, r1 649 | return a; 650 | } 651 | 398: 08 95 ret 652 | 653 | 0000039a : 654 | 655 | // Function for robot velocity control 656 | void velocity (unsigned char left_motor, unsigned char right_motor) 657 | { 658 | OCR5AL = (unsigned char)left_motor; 659 | 39a: 80 93 28 01 sts 0x0128, r24 660 | OCR5BL = (unsigned char)right_motor; 661 | 39e: 60 93 2a 01 sts 0x012A, r22 662 | } 663 | 3a2: 08 95 ret 664 | 665 | 000003a4 : 666 | 667 | void motor_enable (void) 668 | { 669 | PORTL |= 18; // Enable left and right motor. Used with function where velocity is not used 670 | 3a4: eb e0 ldi r30, 0x0B ; 11 671 | 3a6: f1 e0 ldi r31, 0x01 ; 1 672 | 3a8: 80 81 ld r24, Z 673 | 3aa: 82 61 ori r24, 0x12 ; 18 674 | 3ac: 80 83 st Z, r24 675 | } 676 | 3ae: 08 95 ret 677 | 678 | 000003b0 : 679 | void buzzer_on (void) 680 | { 681 | PORTC |= 0x08; 682 | 3b0: 43 9a sbi 0x08, 3 ; 8 683 | } 684 | 3b2: 08 95 ret 685 | 686 | 000003b4 : 687 | 688 | void buzzer_off (void) 689 | { 690 | PORTC &= 0xF7; 691 | 3b4: 43 98 cbi 0x08, 3 ; 8 692 | } 693 | 3b6: 08 95 ret 694 | 695 | 000003b8 : 696 | 697 | void forward (void) 698 | { 699 | //PORTA &= 0xF0; 700 | PORTA = 0x06; 701 | 3b8: 86 e0 ldi r24, 0x06 ; 6 702 | 3ba: 82 b9 out 0x02, r24 ; 2 703 | } 704 | 3bc: 08 95 ret 705 | 706 | 000003be : 707 | 708 | void back (void) 709 | { 710 | //PORTA &= 0xF0; 711 | PORTA = 0x09; 712 | 3be: 89 e0 ldi r24, 0x09 ; 9 713 | 3c0: 82 b9 out 0x02, r24 ; 2 714 | } 715 | 3c2: 08 95 ret 716 | 717 | 000003c4 : 718 | 719 | void left (void) 720 | { 721 | //PORTA &= 0xF0; 722 | PORTA = 0x05; 723 | 3c4: 85 e0 ldi r24, 0x05 ; 5 724 | 3c6: 82 b9 out 0x02, r24 ; 2 725 | } 726 | 3c8: 08 95 ret 727 | 728 | 000003ca : 729 | 730 | void right (void) 731 | { 732 | //PORTA &= 0xF0; 733 | PORTA = 0x0A; 734 | 3ca: 8a e0 ldi r24, 0x0A ; 10 735 | 3cc: 82 b9 out 0x02, r24 ; 2 736 | } 737 | 3ce: 08 95 ret 738 | 739 | 000003d0 : 740 | 741 | void stop (void) 742 | { 743 | PORTA = 0x00; 744 | 3d0: 12 b8 out 0x02, r1 ; 2 745 | } 746 | 3d2: 08 95 ret 747 | 748 | 000003d4 : 749 | void linear_distance_mm(unsigned int DistanceInMM) 750 | { 751 | float ReqdShaftCount = 0; 752 | unsigned long int ReqdShaftCountInt = 0; 753 | 754 | ReqdShaftCount = DistanceInMM / 5.338; // division by resolution to get shaft count 755 | 3d4: bc 01 movw r22, r24 756 | 3d6: 80 e0 ldi r24, 0x00 ; 0 757 | 3d8: 90 e0 ldi r25, 0x00 ; 0 758 | 3da: 0e 94 d1 05 call 0xba2 ; 0xba2 <__floatunsisf> 759 | 3de: 25 ee ldi r18, 0xE5 ; 229 760 | 3e0: 30 ed ldi r19, 0xD0 ; 208 761 | 3e2: 4a ea ldi r20, 0xAA ; 170 762 | 3e4: 50 e4 ldi r21, 0x40 ; 64 763 | 3e6: 0e 94 3d 05 call 0xa7a ; 0xa7a <__divsf3> 764 | ReqdShaftCountInt = (unsigned long int) ReqdShaftCount; 765 | 3ea: 0e 94 a5 05 call 0xb4a ; 0xb4a <__fixunssfsi> 766 | 3ee: 46 2f mov r20, r22 767 | 3f0: 57 2f mov r21, r23 768 | 3f2: 68 2f mov r22, r24 769 | 3f4: 79 2f mov r23, r25 770 | 771 | ShaftCountRight = 0; 772 | 3f6: 10 92 04 02 sts 0x0204, r1 773 | 3fa: 10 92 05 02 sts 0x0205, r1 774 | 3fe: 10 92 06 02 sts 0x0206, r1 775 | 402: 10 92 07 02 sts 0x0207, r1 776 | while(1) 777 | { 778 | if(ShaftCountRight > ReqdShaftCountInt) 779 | 406: 80 91 04 02 lds r24, 0x0204 780 | 40a: 90 91 05 02 lds r25, 0x0205 781 | 40e: a0 91 06 02 lds r26, 0x0206 782 | 412: b0 91 07 02 lds r27, 0x0207 783 | 416: 48 17 cp r20, r24 784 | 418: 59 07 cpc r21, r25 785 | 41a: 6a 07 cpc r22, r26 786 | 41c: 7b 07 cpc r23, r27 787 | 41e: 98 f7 brcc .-26 ; 0x406 788 | { 789 | break; 790 | } 791 | } 792 | stop(); //Stop robot 793 | 420: 0e 94 e8 01 call 0x3d0 ; 0x3d0 794 | } 795 | 424: 08 95 ret 796 | 797 | 00000426 : 798 | 799 | void forward_mm(unsigned int DistanceInMM) 800 | { 801 | 426: cf 93 push r28 802 | 428: df 93 push r29 803 | 42a: ec 01 movw r28, r24 804 | forward(); 805 | 42c: 0e 94 dc 01 call 0x3b8 ; 0x3b8 806 | linear_distance_mm(DistanceInMM); 807 | 430: ce 01 movw r24, r28 808 | 432: 0e 94 ea 01 call 0x3d4 ; 0x3d4 809 | } 810 | 436: df 91 pop r29 811 | 438: cf 91 pop r28 812 | 43a: 08 95 ret 813 | 814 | 0000043c : 815 | 816 | void back_mm(unsigned int DistanceInMM) 817 | { 818 | 43c: cf 93 push r28 819 | 43e: df 93 push r29 820 | 440: ec 01 movw r28, r24 821 | back(); 822 | 442: 0e 94 df 01 call 0x3be ; 0x3be 823 | linear_distance_mm(DistanceInMM); 824 | 446: ce 01 movw r24, r28 825 | 448: 0e 94 ea 01 call 0x3d4 ; 0x3d4 826 | } 827 | 44c: df 91 pop r29 828 | 44e: cf 91 pop r28 829 | 450: 08 95 ret 830 | 831 | 00000452 : 832 | //Function to rotate Servo 1 by a specified angle in the multiples of 1.86 degrees 833 | void servo_1(unsigned char degrees) 834 | { 835 | float PositionPanServo = 0; 836 | PositionPanServo = ((float)degrees / 1.86) + 35.0; 837 | OCR1AH = 0x00; 838 | 452: 10 92 89 00 sts 0x0089, r1 839 | 840 | //Function to rotate Servo 1 by a specified angle in the multiples of 1.86 degrees 841 | void servo_1(unsigned char degrees) 842 | { 843 | float PositionPanServo = 0; 844 | PositionPanServo = ((float)degrees / 1.86) + 35.0; 845 | 456: 68 2f mov r22, r24 846 | 458: 70 e0 ldi r23, 0x00 ; 0 847 | 45a: 80 e0 ldi r24, 0x00 ; 0 848 | 45c: 90 e0 ldi r25, 0x00 ; 0 849 | 45e: 0e 94 d1 05 call 0xba2 ; 0xba2 <__floatunsisf> 850 | 462: 2b e7 ldi r18, 0x7B ; 123 851 | 464: 34 e1 ldi r19, 0x14 ; 20 852 | 466: 4e ee ldi r20, 0xEE ; 238 853 | 468: 5f e3 ldi r21, 0x3F ; 63 854 | 46a: 0e 94 3d 05 call 0xa7a ; 0xa7a <__divsf3> 855 | 46e: 20 e0 ldi r18, 0x00 ; 0 856 | 470: 30 e0 ldi r19, 0x00 ; 0 857 | 472: 4c e0 ldi r20, 0x0C ; 12 858 | 474: 52 e4 ldi r21, 0x42 ; 66 859 | 476: 0e 94 d9 04 call 0x9b2 ; 0x9b2 <__addsf3> 860 | OCR1AH = 0x00; 861 | OCR1AL = (unsigned char) PositionPanServo; 862 | 47a: 0e 94 a5 05 call 0xb4a ; 0xb4a <__fixunssfsi> 863 | 47e: 60 93 88 00 sts 0x0088, r22 864 | } 865 | 482: 08 95 ret 866 | 867 | 00000484 : 868 | //Function to rotate Servo 2 by a specified angle in the multiples of 1.86 degrees 869 | void servo_2(unsigned char degrees) 870 | { 871 | float PositionTiltServo = 0; 872 | PositionTiltServo = ((float)degrees / 1.86) + 35.0; 873 | OCR1BH = 0x00; 874 | 484: 10 92 8b 00 sts 0x008B, r1 875 | 876 | //Function to rotate Servo 2 by a specified angle in the multiples of 1.86 degrees 877 | void servo_2(unsigned char degrees) 878 | { 879 | float PositionTiltServo = 0; 880 | PositionTiltServo = ((float)degrees / 1.86) + 35.0; 881 | 488: 68 2f mov r22, r24 882 | 48a: 70 e0 ldi r23, 0x00 ; 0 883 | 48c: 80 e0 ldi r24, 0x00 ; 0 884 | 48e: 90 e0 ldi r25, 0x00 ; 0 885 | 490: 0e 94 d1 05 call 0xba2 ; 0xba2 <__floatunsisf> 886 | 494: 2b e7 ldi r18, 0x7B ; 123 887 | 496: 34 e1 ldi r19, 0x14 ; 20 888 | 498: 4e ee ldi r20, 0xEE ; 238 889 | 49a: 5f e3 ldi r21, 0x3F ; 63 890 | 49c: 0e 94 3d 05 call 0xa7a ; 0xa7a <__divsf3> 891 | 4a0: 20 e0 ldi r18, 0x00 ; 0 892 | 4a2: 30 e0 ldi r19, 0x00 ; 0 893 | 4a4: 4c e0 ldi r20, 0x0C ; 12 894 | 4a6: 52 e4 ldi r21, 0x42 ; 66 895 | 4a8: 0e 94 d9 04 call 0x9b2 ; 0x9b2 <__addsf3> 896 | OCR1BH = 0x00; 897 | OCR1BL = (unsigned char) PositionTiltServo; 898 | 4ac: 0e 94 a5 05 call 0xb4a ; 0xb4a <__fixunssfsi> 899 | 4b0: 60 93 8a 00 sts 0x008A, r22 900 | } 901 | 4b4: 08 95 ret 902 | 903 | 000004b6 : 904 | //Function to rotate Servo 3 by a specified angle in the multiples of 1.86 degrees 905 | void servo_3(unsigned char degrees) 906 | { 907 | float PositionServo = 0; 908 | PositionServo = ((float)degrees / 1.86) + 35.0; 909 | OCR1CH = 0x00; 910 | 4b6: 10 92 8d 00 sts 0x008D, r1 911 | 912 | //Function to rotate Servo 3 by a specified angle in the multiples of 1.86 degrees 913 | void servo_3(unsigned char degrees) 914 | { 915 | float PositionServo = 0; 916 | PositionServo = ((float)degrees / 1.86) + 35.0; 917 | 4ba: 68 2f mov r22, r24 918 | 4bc: 70 e0 ldi r23, 0x00 ; 0 919 | 4be: 80 e0 ldi r24, 0x00 ; 0 920 | 4c0: 90 e0 ldi r25, 0x00 ; 0 921 | 4c2: 0e 94 d1 05 call 0xba2 ; 0xba2 <__floatunsisf> 922 | 4c6: 2b e7 ldi r18, 0x7B ; 123 923 | 4c8: 34 e1 ldi r19, 0x14 ; 20 924 | 4ca: 4e ee ldi r20, 0xEE ; 238 925 | 4cc: 5f e3 ldi r21, 0x3F ; 63 926 | 4ce: 0e 94 3d 05 call 0xa7a ; 0xa7a <__divsf3> 927 | 4d2: 20 e0 ldi r18, 0x00 ; 0 928 | 4d4: 30 e0 ldi r19, 0x00 ; 0 929 | 4d6: 4c e0 ldi r20, 0x0C ; 12 930 | 4d8: 52 e4 ldi r21, 0x42 ; 66 931 | 4da: 0e 94 d9 04 call 0x9b2 ; 0x9b2 <__addsf3> 932 | OCR1CH = 0x00; 933 | OCR1CL = (unsigned char) PositionServo; 934 | 4de: 0e 94 a5 05 call 0xb4a ; 0xb4a <__fixunssfsi> 935 | 4e2: 60 93 8c 00 sts 0x008C, r22 936 | } 937 | 4e6: 08 95 ret 938 | 939 | 000004e8 : 940 | //and make them free by giving 100% duty cycle at the PWM. This function can be used to 941 | //reduce the power consumption of the motor if it is holding load against the gravity. 942 | 943 | void servo_1_free (void) //makes servo 1 free rotating 944 | { 945 | OCR1AH = 0x03; 946 | 4e8: 83 e0 ldi r24, 0x03 ; 3 947 | 4ea: 80 93 89 00 sts 0x0089, r24 948 | OCR1AL = 0xFF; //Servo 1 off 949 | 4ee: 8f ef ldi r24, 0xFF ; 255 950 | 4f0: 80 93 88 00 sts 0x0088, r24 951 | } 952 | 4f4: 08 95 ret 953 | 954 | 000004f6 : 955 | 956 | void servo_2_free (void) //makes servo 2 free rotating 957 | { 958 | OCR1BH = 0x03; 959 | 4f6: 83 e0 ldi r24, 0x03 ; 3 960 | 4f8: 80 93 8b 00 sts 0x008B, r24 961 | OCR1BL = 0xFF; //Servo 2 off 962 | 4fc: 8f ef ldi r24, 0xFF ; 255 963 | 4fe: 80 93 8a 00 sts 0x008A, r24 964 | } 965 | 502: 08 95 ret 966 | 967 | 00000504 : 968 | 969 | void servo_3_free (void) //makes servo 3 free rotating 970 | { 971 | OCR1CH = 0x03; 972 | 504: 83 e0 ldi r24, 0x03 ; 3 973 | 506: 80 93 8d 00 sts 0x008D, r24 974 | OCR1CL = 0xFF; //Servo 3 off 975 | 50a: 8f ef ldi r24, 0xFF ; 255 976 | 50c: 80 93 8c 00 sts 0x008C, r24 977 | } 978 | 510: 08 95 ret 979 | 980 | 00000512 : 981 | 982 | void LED_bargraph_on (unsigned char data) 983 | { 984 | PORTJ |= data; 985 | 512: e5 e0 ldi r30, 0x05 ; 5 986 | 514: f1 e0 ldi r31, 0x01 ; 1 987 | 516: 90 81 ld r25, Z 988 | 518: 89 2b or r24, r25 989 | 51a: 80 83 st Z, r24 990 | } 991 | 51c: 08 95 ret 992 | 993 | 0000051e : 994 | 995 | void LED_bargraph_off (unsigned char data) 996 | { 997 | PORTJ &= data; 998 | 51e: e5 e0 ldi r30, 0x05 ; 5 999 | 520: f1 e0 ldi r31, 0x01 ; 1 1000 | 522: 90 81 ld r25, Z 1001 | 524: 89 23 and r24, r25 1002 | 526: 80 83 st Z, r24 1003 | } 1004 | 528: 08 95 ret 1005 | 1006 | 0000052a : 1007 | //------------------------------------------------------------------------------- 1008 | unsigned char spi_master_tx_and_rx (unsigned char data) 1009 | { 1010 | unsigned char rx_data = 0; 1011 | 1012 | PORTB = PORTB & 0xFE; // make SS pin low 1013 | 52a: 28 98 cbi 0x05, 0 ; 5 1014 | SPDR = data; 1015 | 52c: 8e bd out 0x2e, r24 ; 46 1016 | while(!(SPSR & (1< 1020 | #else 1021 | //round up by default 1022 | __ticks_dc = (uint32_t)(ceil(fabs(__tmp))); 1023 | #endif 1024 | 1025 | __builtin_avr_delay_cycles(__ticks_dc); 1026 | 534: 86 e6 ldi r24, 0x66 ; 102 1027 | 536: 9e e0 ldi r25, 0x0E ; 14 1028 | 538: 01 97 sbiw r24, 0x01 ; 1 1029 | 53a: f1 f7 brne .-4 ; 0x538 1030 | 53c: 00 00 nop 1031 | 1032 | _delay_ms(1); //time for ADC conversion in the slave microcontroller 1033 | 1034 | SPDR = 0x50; // send dummy byte to read back data from the slave microcontroller 1035 | 53e: 80 e5 ldi r24, 0x50 ; 80 1036 | 540: 8e bd out 0x2e, r24 ; 46 1037 | while(!(SPSR & (1< 1041 | rx_data = SPDR; 1042 | 548: 8e b5 in r24, 0x2e ; 46 1043 | PORTB = PORTB | 0x01; // make SS high 1044 | 54a: 28 9a sbi 0x05, 0 ; 5 1045 | return rx_data; 1046 | } 1047 | 54c: 08 95 ret 1048 | 1049 | 0000054e <__vector_51>: 1050 | 1051 | 1052 | 1053 | //SIGNAL(SIG_USART2_RECV) // ISR for receive complete interrupt 1054 | ISR(USART2_RX_vect) 1055 | { 1056 | 54e: 1f 92 push r1 1057 | 550: 0f 92 push r0 1058 | 552: 0f b6 in r0, 0x3f ; 63 1059 | 554: 0f 92 push r0 1060 | 556: 0b b6 in r0, 0x3b ; 59 1061 | 558: 0f 92 push r0 1062 | 55a: 11 24 eor r1, r1 1063 | 55c: 2f 93 push r18 1064 | 55e: 3f 93 push r19 1065 | 560: 8f 93 push r24 1066 | 562: 9f 93 push r25 1067 | 564: af 93 push r26 1068 | 566: bf 93 push r27 1069 | 568: ef 93 push r30 1070 | 56a: ff 93 push r31 1071 | rec_data = UDR2; //making copy of data from UDR2 in 'data' variable 1072 | 56c: 90 91 d6 00 lds r25, 0x00D6 1073 | 570: 90 93 4d 02 sts 0x024D, r25 1074 | 1075 | while(!(UCSR2A && (1< 1081 | 1082 | if (data_packet_received == 0) 1083 | 57e: 80 91 17 02 lds r24, 0x0217 1084 | 582: 88 23 and r24, r24 1085 | 584: 09 f0 breq .+2 ; 0x588 <__vector_51+0x3a> 1086 | 586: 47 c0 rjmp .+142 ; 0x616 <__vector_51+0xc8> 1087 | { 1088 | if (rec_data == '\n' ) // '\n' decimal value is 10 1089 | 588: 9a 30 cpi r25, 0x0A ; 10 1090 | 58a: 71 f4 brne .+28 ; 0x5a8 <__vector_51+0x5a> 1091 | { 1092 | //state = _second_last_byte 1093 | uart_data_buff[i] = rec_data; 1094 | 58c: 80 91 19 02 lds r24, 0x0219 1095 | 590: e4 e3 ldi r30, 0x34 ; 52 1096 | 592: f2 e0 ldi r31, 0x02 ; 2 1097 | 594: e8 0f add r30, r24 1098 | 596: f1 1d adc r31, r1 1099 | 598: 90 83 st Z, r25 1100 | i++; 1101 | 59a: 8f 5f subi r24, 0xFF ; 255 1102 | 59c: 80 93 19 02 sts 0x0219, r24 1103 | end_char_rec = 1; 1104 | 5a0: 81 e0 ldi r24, 0x01 ; 1 1105 | 5a2: 80 93 1a 02 sts 0x021A, r24 1106 | 5a6: 37 c0 rjmp .+110 ; 0x616 <__vector_51+0xc8> 1107 | // UDR2 = rec_data; 1108 | } 1109 | 1110 | else 1111 | { 1112 | if((end_char_rec == 1) && (rec_data == '\r')) //'\r' indicates end of transmission. It should come after '\n' 1113 | 5a8: 80 91 1a 02 lds r24, 0x021A 1114 | 5ac: 81 30 cpi r24, 0x01 ; 1 1115 | 5ae: 49 f5 brne .+82 ; 0x602 <__vector_51+0xb4> 1116 | 5b0: 9d 30 cpi r25, 0x0D ; 13 1117 | 5b2: 89 f5 brne .+98 ; 0x616 <__vector_51+0xc8> 1118 | { 1119 | uart_data_buff[i] = rec_data; 1120 | 5b4: 20 91 19 02 lds r18, 0x0219 1121 | 5b8: e4 e3 ldi r30, 0x34 ; 52 1122 | 5ba: f2 e0 ldi r31, 0x02 ; 2 1123 | 5bc: e2 0f add r30, r18 1124 | 5be: f1 1d adc r31, r1 1125 | 5c0: 8d e0 ldi r24, 0x0D ; 13 1126 | 5c2: 80 83 st Z, r24 1127 | i++; 1128 | 5c4: 32 2f mov r19, r18 1129 | 5c6: 3f 5f subi r19, 0xFF ; 255 1130 | 5c8: 30 93 19 02 sts 0x0219, r19 1131 | end_char_rec = 2; 1132 | 5cc: 82 e0 ldi r24, 0x02 ; 2 1133 | 5ce: 80 93 1a 02 sts 0x021A, r24 1134 | data_packet_received = 1; 1135 | 5d2: 81 e0 ldi r24, 0x01 ; 1 1136 | 5d4: 80 93 17 02 sts 0x0217, r24 1137 | 1138 | for (j = 0;j 1142 | 5e0: eb e1 ldi r30, 0x1B ; 27 1143 | 5e2: f2 e0 ldi r31, 0x02 ; 2 1144 | 5e4: a4 e3 ldi r26, 0x34 ; 52 1145 | 5e6: b2 e0 ldi r27, 0x02 ; 2 1146 | } 1147 | 1148 | 1149 | 1150 | //SIGNAL(SIG_USART2_RECV) // ISR for receive complete interrupt 1151 | ISR(USART2_RX_vect) 1152 | 5e8: cf 01 movw r24, r30 1153 | 5ea: 01 96 adiw r24, 0x01 ; 1 1154 | 5ec: 82 0f add r24, r18 1155 | 5ee: 91 1d adc r25, r1 1156 | end_char_rec = 2; 1157 | data_packet_received = 1; 1158 | 1159 | for (j = 0;j 1176 | 5fc: 30 93 18 02 sts 0x0218, r19 1177 | 600: 0a c0 rjmp .+20 ; 0x616 <__vector_51+0xc8> 1178 | // discard the data and check 1179 | } 1180 | 1181 | else // store other data bytes 1182 | { 1183 | uart_data_buff[i] = rec_data; 1184 | 602: 80 91 19 02 lds r24, 0x0219 1185 | 606: e4 e3 ldi r30, 0x34 ; 52 1186 | 608: f2 e0 ldi r31, 0x02 ; 2 1187 | 60a: e8 0f add r30, r24 1188 | 60c: f1 1d adc r31, r1 1189 | 60e: 90 83 st Z, r25 1190 | i++; 1191 | 610: 8f 5f subi r24, 0xFF ; 255 1192 | 612: 80 93 19 02 sts 0x0219, r24 1193 | // UDR2 = rec_data; 1194 | } 1195 | } 1196 | } 1197 | } // end of ISR 1198 | 616: ff 91 pop r31 1199 | 618: ef 91 pop r30 1200 | 61a: bf 91 pop r27 1201 | 61c: af 91 pop r26 1202 | 61e: 9f 91 pop r25 1203 | 620: 8f 91 pop r24 1204 | 622: 3f 91 pop r19 1205 | 624: 2f 91 pop r18 1206 | 626: 0f 90 pop r0 1207 | 628: 0b be out 0x3b, r0 ; 59 1208 | 62a: 0f 90 pop r0 1209 | 62c: 0f be out 0x3f, r0 ; 63 1210 | 62e: 0f 90 pop r0 1211 | 630: 1f 90 pop r1 1212 | 632: 18 95 reti 1213 | 1214 | 00000634 : 1215 | 1216 | void send_sensor_data(void) 1217 | { 1218 | if (device_id == 0x00) 1219 | 634: 80 91 15 02 lds r24, 0x0215 1220 | 638: 88 23 and r24, r24 1221 | 63a: 09 f0 breq .+2 ; 0x63e 1222 | 63c: 6a c0 rjmp .+212 ; 0x712 1223 | { 1224 | if (function_type == 0x00) 1225 | 63e: 80 91 13 02 lds r24, 0x0213 1226 | 642: 88 23 and r24, r24 1227 | 644: 29 f4 brne .+10 ; 0x650 1228 | { 1229 | UDR2 = ADC_Conversion(0); // Battery Voltage 1230 | 646: 0e 94 b0 01 call 0x360 ; 0x360 1231 | 64a: 80 93 d6 00 sts 0x00D6, r24 1232 | 64e: 61 c0 rjmp .+194 ; 0x712 1233 | } 1234 | 1235 | else if (function_type == 0x01) 1236 | 650: 81 30 cpi r24, 0x01 ; 1 1237 | 652: 29 f4 brne .+10 ; 0x65e 1238 | { 1239 | UDR2 = ADC_Conversion(1); // right WL sensor 1240 | 654: 0e 94 b0 01 call 0x360 ; 0x360 1241 | 658: 80 93 d6 00 sts 0x00D6, r24 1242 | 65c: 5a c0 rjmp .+180 ; 0x712 1243 | } 1244 | 1245 | else if (function_type == 0x02) 1246 | 65e: 82 30 cpi r24, 0x02 ; 2 1247 | 660: 29 f4 brne .+10 ; 0x66c 1248 | { 1249 | UDR2 = ADC_Conversion(2); // Center WL sensor 1250 | 662: 0e 94 b0 01 call 0x360 ; 0x360 1251 | 666: 80 93 d6 00 sts 0x00D6, r24 1252 | 66a: 53 c0 rjmp .+166 ; 0x712 1253 | } 1254 | 1255 | else if (function_type == 0x03) 1256 | 66c: 83 30 cpi r24, 0x03 ; 3 1257 | 66e: 29 f4 brne .+10 ; 0x67a 1258 | { 1259 | UDR2 = ADC_Conversion(3); // left WL sensor 1260 | 670: 0e 94 b0 01 call 0x360 ; 0x360 1261 | 674: 80 93 d6 00 sts 0x00D6, r24 1262 | 678: 4c c0 rjmp .+152 ; 0x712 1263 | } 1264 | 1265 | else if (function_type == 0x04) 1266 | 67a: 84 30 cpi r24, 0x04 ; 4 1267 | 67c: 29 f4 brne .+10 ; 0x688 1268 | { 1269 | UDR2 = ADC_Conversion(4); // IR Proximity sensor-1 1270 | 67e: 0e 94 b0 01 call 0x360 ; 0x360 1271 | 682: 80 93 d6 00 sts 0x00D6, r24 1272 | 686: 45 c0 rjmp .+138 ; 0x712 1273 | } 1274 | 1275 | else if (function_type == 0x05) 1276 | 688: 85 30 cpi r24, 0x05 ; 5 1277 | 68a: 29 f4 brne .+10 ; 0x696 1278 | { 1279 | UDR2 = ADC_Conversion(5); // IR Proximity sensor-2 1280 | 68c: 0e 94 b0 01 call 0x360 ; 0x360 1281 | 690: 80 93 d6 00 sts 0x00D6, r24 1282 | 694: 3e c0 rjmp .+124 ; 0x712 1283 | } 1284 | 1285 | else if (function_type == 0x06) 1286 | 696: 86 30 cpi r24, 0x06 ; 6 1287 | 698: 29 f4 brne .+10 ; 0x6a4 1288 | { 1289 | UDR2 = ADC_Conversion(6); // IR Proximity sensor-3 1290 | 69a: 0e 94 b0 01 call 0x360 ; 0x360 1291 | 69e: 80 93 d6 00 sts 0x00D6, r24 1292 | 6a2: 37 c0 rjmp .+110 ; 0x712 1293 | } 1294 | 1295 | else if (function_type == 0x07) 1296 | 6a4: 87 30 cpi r24, 0x07 ; 7 1297 | 6a6: 29 f4 brne .+10 ; 0x6b2 1298 | { 1299 | UDR2 = ADC_Conversion(7); // IR Proximity sensor-4 1300 | 6a8: 0e 94 b0 01 call 0x360 ; 0x360 1301 | 6ac: 80 93 d6 00 sts 0x00D6, r24 1302 | 6b0: 30 c0 rjmp .+96 ; 0x712 1303 | } 1304 | 1305 | else if (function_type == 0x08) 1306 | 6b2: 88 30 cpi r24, 0x08 ; 8 1307 | 6b4: 29 f4 brne .+10 ; 0x6c0 1308 | { 1309 | UDR2 = ADC_Conversion(8); // IR Proximity sensor-5 1310 | 6b6: 0e 94 b0 01 call 0x360 ; 0x360 1311 | 6ba: 80 93 d6 00 sts 0x00D6, r24 1312 | 6be: 29 c0 rjmp .+82 ; 0x712 1313 | } 1314 | 1315 | else if (function_type == 0x09) 1316 | 6c0: 89 30 cpi r24, 0x09 ; 9 1317 | 6c2: 29 f4 brne .+10 ; 0x6ce 1318 | { 1319 | UDR2 = ADC_Conversion(9); // Sharp Sensor-1 1320 | 6c4: 0e 94 b0 01 call 0x360 ; 0x360 1321 | 6c8: 80 93 d6 00 sts 0x00D6, r24 1322 | 6cc: 22 c0 rjmp .+68 ; 0x712 1323 | } 1324 | 1325 | else if (function_type == 0x0A) 1326 | 6ce: 8a 30 cpi r24, 0x0A ; 10 1327 | 6d0: 29 f4 brne .+10 ; 0x6dc 1328 | { 1329 | UDR2 = ADC_Conversion(10); // Sharp Sensor-2 1330 | 6d2: 0e 94 b0 01 call 0x360 ; 0x360 1331 | 6d6: 80 93 d6 00 sts 0x00D6, r24 1332 | 6da: 1b c0 rjmp .+54 ; 0x712 1333 | } 1334 | 1335 | else if (function_type == 0x0B) 1336 | 6dc: 8b 30 cpi r24, 0x0B ; 11 1337 | 6de: 29 f4 brne .+10 ; 0x6ea 1338 | { 1339 | UDR2 = ADC_Conversion(11); // Sharp Sensor-3 1340 | 6e0: 0e 94 b0 01 call 0x360 ; 0x360 1341 | 6e4: 80 93 d6 00 sts 0x00D6, r24 1342 | 6e8: 14 c0 rjmp .+40 ; 0x712 1343 | } 1344 | 1345 | else if (function_type == 0x0C) 1346 | 6ea: 8c 30 cpi r24, 0x0C ; 12 1347 | 6ec: 29 f4 brne .+10 ; 0x6f8 1348 | { 1349 | UDR2 = ADC_Conversion(12); // Sharp Sensor-4 1350 | 6ee: 0e 94 b0 01 call 0x360 ; 0x360 1351 | 6f2: 80 93 d6 00 sts 0x00D6, r24 1352 | 6f6: 0d c0 rjmp .+26 ; 0x712 1353 | } 1354 | 1355 | else if (function_type == 0x0D) 1356 | 6f8: 8d 30 cpi r24, 0x0D ; 13 1357 | 6fa: 29 f4 brne .+10 ; 0x706 1358 | { 1359 | UDR2 = ADC_Conversion(13); // Sharp Sensor-5 1360 | 6fc: 0e 94 b0 01 call 0x360 ; 0x360 1361 | 700: 80 93 d6 00 sts 0x00D6, r24 1362 | 704: 06 c0 rjmp .+12 ; 0x712 1363 | } 1364 | 1365 | else if (function_type == 0x0E) 1366 | 706: 8e 30 cpi r24, 0x0E ; 14 1367 | 708: 21 f4 brne .+8 ; 0x712 1368 | { 1369 | UDR2 = ADC_Conversion(14); // Connected to servo pod 1370 | 70a: 0e 94 b0 01 call 0x360 ; 0x360 1371 | 70e: 80 93 d6 00 sts 0x00D6, r24 1372 | { 1373 | UDR2 = ADC_Conversion(15); // Connected to servo pod 1374 | } 1375 | } 1376 | 1377 | if (device_id == 0x01) // ATmega 8 ADC 1378 | 712: 80 91 15 02 lds r24, 0x0215 1379 | 716: 81 30 cpi r24, 0x01 ; 1 1380 | 718: c9 f5 brne .+114 ; 0x78c 1381 | { 1382 | if (function_type == 0x00) // Whiteline sensor 4 1383 | 71a: 80 91 13 02 lds r24, 0x0213 1384 | 71e: 88 23 and r24, r24 1385 | 720: 29 f4 brne .+10 ; 0x72c 1386 | { 1387 | UDR2 = spi_master_tx_and_rx(0); 1388 | 722: 0e 94 95 02 call 0x52a ; 0x52a 1389 | 726: 80 93 d6 00 sts 0x00D6, r24 1390 | 72a: 08 95 ret 1391 | } 1392 | 1393 | else if (function_type == 0x01) // Whiteline sensor 5 1394 | 72c: 81 30 cpi r24, 0x01 ; 1 1395 | 72e: 29 f4 brne .+10 ; 0x73a 1396 | { 1397 | UDR2 = spi_master_tx_and_rx(1); 1398 | 730: 0e 94 95 02 call 0x52a ; 0x52a 1399 | 734: 80 93 d6 00 sts 0x00D6, r24 1400 | 738: 08 95 ret 1401 | } 1402 | 1403 | else if (function_type == 0x02) // Whiteline sensor 6 1404 | 73a: 82 30 cpi r24, 0x02 ; 2 1405 | 73c: 29 f4 brne .+10 ; 0x748 1406 | { 1407 | UDR2 = spi_master_tx_and_rx(2); 1408 | 73e: 0e 94 95 02 call 0x52a ; 0x52a 1409 | 742: 80 93 d6 00 sts 0x00D6, r24 1410 | 746: 08 95 ret 1411 | } 1412 | 1413 | else if (function_type == 0x03) // Whiteline sensor 7 1414 | 748: 83 30 cpi r24, 0x03 ; 3 1415 | 74a: 29 f4 brne .+10 ; 0x756 1416 | { 1417 | UDR2 = spi_master_tx_and_rx(3); 1418 | 74c: 0e 94 95 02 call 0x52a ; 0x52a 1419 | 750: 80 93 d6 00 sts 0x00D6, r24 1420 | 754: 08 95 ret 1421 | } 1422 | 1423 | else if (function_type == 0x04) // Current Sensor 1424 | 756: 84 30 cpi r24, 0x04 ; 4 1425 | 758: 29 f4 brne .+10 ; 0x764 1426 | { 1427 | UDR2 = spi_master_tx_and_rx(4); 1428 | 75a: 0e 94 95 02 call 0x52a ; 0x52a 1429 | 75e: 80 93 d6 00 sts 0x00D6, r24 1430 | 762: 08 95 ret 1431 | } 1432 | 1433 | else if (function_type == 0x05) // IR proximity sensor 6 1434 | 764: 85 30 cpi r24, 0x05 ; 5 1435 | 766: 29 f4 brne .+10 ; 0x772 1436 | { 1437 | UDR2 = spi_master_tx_and_rx(5); 1438 | 768: 0e 94 95 02 call 0x52a ; 0x52a 1439 | 76c: 80 93 d6 00 sts 0x00D6, r24 1440 | 770: 08 95 ret 1441 | } 1442 | 1443 | else if (function_type == 0x06) // IR proximity sensor 7 1444 | 772: 86 30 cpi r24, 0x06 ; 6 1445 | 774: 29 f4 brne .+10 ; 0x780 1446 | { 1447 | UDR2 = spi_master_tx_and_rx(6); 1448 | 776: 0e 94 95 02 call 0x52a ; 0x52a 1449 | 77a: 80 93 d6 00 sts 0x00D6, r24 1450 | 77e: 08 95 ret 1451 | } 1452 | 1453 | else if (function_type == 0x07) // IR proximity sensor 8 1454 | 780: 87 30 cpi r24, 0x07 ; 7 1455 | 782: 21 f4 brne .+8 ; 0x78c 1456 | { 1457 | UDR2 = spi_master_tx_and_rx(7); 1458 | 784: 0e 94 95 02 call 0x52a ; 0x52a 1459 | 788: 80 93 d6 00 sts 0x00D6, r24 1460 | 78c: 08 95 ret 1461 | 1462 | 0000078e : 1463 | 1464 | } 1465 | 1466 | void actuate_devices(void) 1467 | { 1468 | if (device_id == 0x01) // Buzzer has device id = 1 1469 | 78e: 80 91 15 02 lds r24, 0x0215 1470 | 792: 81 30 cpi r24, 0x01 ; 1 1471 | 794: 59 f4 brne .+22 ; 0x7ac 1472 | { 1473 | if (function_type == 0x00) 1474 | 796: 80 91 13 02 lds r24, 0x0213 1475 | 79a: 88 23 and r24, r24 1476 | 79c: 19 f4 brne .+6 ; 0x7a4 1477 | { 1478 | buzzer_on(); 1479 | 79e: 0e 94 d8 01 call 0x3b0 ; 0x3b0 1480 | 7a2: 04 c0 rjmp .+8 ; 0x7ac 1481 | } 1482 | else if (function_type == 0x01) 1483 | 7a4: 81 30 cpi r24, 0x01 ; 1 1484 | 7a6: 11 f4 brne .+4 ; 0x7ac 1485 | { 1486 | buzzer_off(); 1487 | 7a8: 0e 94 da 01 call 0x3b4 ; 0x3b4 1488 | } 1489 | } 1490 | 1491 | if (device_id == 0x02) // Motor has device id = 2 1492 | 7ac: 80 91 15 02 lds r24, 0x0215 1493 | 7b0: 82 30 cpi r24, 0x02 ; 2 1494 | 7b2: 69 f5 brne .+90 ; 0x80e 1495 | { 1496 | if (function_type == 0x00) 1497 | 7b4: 80 91 13 02 lds r24, 0x0213 1498 | 7b8: 88 23 and r24, r24 1499 | 7ba: 29 f4 brne .+10 ; 0x7c6 1500 | { 1501 | motor_enable(); 1502 | 7bc: 0e 94 d2 01 call 0x3a4 ; 0x3a4 1503 | forward(); 1504 | 7c0: 0e 94 dc 01 call 0x3b8 ; 0x3b8 1505 | 7c4: 24 c0 rjmp .+72 ; 0x80e 1506 | } 1507 | else if (function_type == 0x01) 1508 | 7c6: 81 30 cpi r24, 0x01 ; 1 1509 | 7c8: 29 f4 brne .+10 ; 0x7d4 1510 | { 1511 | motor_enable(); 1512 | 7ca: 0e 94 d2 01 call 0x3a4 ; 0x3a4 1513 | back(); 1514 | 7ce: 0e 94 df 01 call 0x3be ; 0x3be 1515 | 7d2: 1d c0 rjmp .+58 ; 0x80e 1516 | } 1517 | else if (function_type == 0x02) 1518 | 7d4: 82 30 cpi r24, 0x02 ; 2 1519 | 7d6: 29 f4 brne .+10 ; 0x7e2 1520 | { 1521 | motor_enable(); 1522 | 7d8: 0e 94 d2 01 call 0x3a4 ; 0x3a4 1523 | right(); 1524 | 7dc: 0e 94 e5 01 call 0x3ca ; 0x3ca 1525 | 7e0: 16 c0 rjmp .+44 ; 0x80e 1526 | } 1527 | else if (function_type == 0x03) 1528 | 7e2: 83 30 cpi r24, 0x03 ; 3 1529 | 7e4: 29 f4 brne .+10 ; 0x7f0 1530 | { 1531 | motor_enable(); 1532 | 7e6: 0e 94 d2 01 call 0x3a4 ; 0x3a4 1533 | left(); 1534 | 7ea: 0e 94 e2 01 call 0x3c4 ; 0x3c4 1535 | 7ee: 0f c0 rjmp .+30 ; 0x80e 1536 | } 1537 | else if (function_type == 0x04) 1538 | 7f0: 84 30 cpi r24, 0x04 ; 4 1539 | 7f2: 29 f4 brne .+10 ; 0x7fe 1540 | { 1541 | motor_enable(); 1542 | 7f4: 0e 94 d2 01 call 0x3a4 ; 0x3a4 1543 | stop(); 1544 | 7f8: 0e 94 e8 01 call 0x3d0 ; 0x3d0 1545 | 7fc: 08 c0 rjmp .+16 ; 0x80e 1546 | } 1547 | else if (function_type == 0x09) 1548 | 7fe: 89 30 cpi r24, 0x09 ; 9 1549 | 800: 31 f4 brne .+12 ; 0x80e 1550 | { 1551 | //forward(); 1552 | //UDR2 = param_1; 1553 | velocity(param_1,param_2); 1554 | 802: 80 91 10 02 lds r24, 0x0210 1555 | 806: 60 91 0e 02 lds r22, 0x020E 1556 | 80a: 0e 94 cd 01 call 0x39a ; 0x39a 1557 | 1558 | } 1559 | } 1560 | 1561 | if (device_id == 0x03) // position encoder has device id = 3 1562 | 80e: 80 91 15 02 lds r24, 0x0215 1563 | 812: 83 30 cpi r24, 0x03 ; 3 1564 | 814: a1 f4 brne .+40 ; 0x83e 1565 | { 1566 | if (function_type == 0x00) 1567 | 816: 80 91 13 02 lds r24, 0x0213 1568 | 81a: 88 23 and r24, r24 1569 | 81c: 31 f4 brne .+12 ; 0x82a 1570 | { 1571 | forward_mm(param_1); 1572 | 81e: 80 91 10 02 lds r24, 0x0210 1573 | 822: 90 91 11 02 lds r25, 0x0211 1574 | 826: 0e 94 13 02 call 0x426 ; 0x426 1575 | } 1576 | 1577 | if (function_type == 0x01 ) 1578 | 82a: 80 91 13 02 lds r24, 0x0213 1579 | 82e: 81 30 cpi r24, 0x01 ; 1 1580 | 830: 31 f4 brne .+12 ; 0x83e 1581 | { 1582 | back_mm(param_1); 1583 | 832: 80 91 10 02 lds r24, 0x0210 1584 | 836: 90 91 11 02 lds r25, 0x0211 1585 | 83a: 0e 94 1e 02 call 0x43c ; 0x43c 1586 | } 1587 | } 1588 | 1589 | if (device_id == 0x04) // servo has device id = 4 1590 | 83e: 80 91 15 02 lds r24, 0x0215 1591 | 842: 84 30 cpi r24, 0x04 ; 4 1592 | 844: 51 f5 brne .+84 ; 0x89a 1593 | { 1594 | if (function_type == 0x00) 1595 | 846: 80 91 13 02 lds r24, 0x0213 1596 | 84a: 88 23 and r24, r24 1597 | 84c: 21 f4 brne .+8 ; 0x856 1598 | { 1599 | servo_1(param_1); 1600 | 84e: 80 91 10 02 lds r24, 0x0210 1601 | 852: 0e 94 29 02 call 0x452 ; 0x452 1602 | } 1603 | 1604 | if (function_type == 0x01 ) 1605 | 856: 80 91 13 02 lds r24, 0x0213 1606 | 85a: 81 30 cpi r24, 0x01 ; 1 1607 | 85c: 21 f4 brne .+8 ; 0x866 1608 | { 1609 | servo_2(param_1); 1610 | 85e: 80 91 10 02 lds r24, 0x0210 1611 | 862: 0e 94 42 02 call 0x484 ; 0x484 1612 | } 1613 | 1614 | if (function_type == 0x02 ) 1615 | 866: 80 91 13 02 lds r24, 0x0213 1616 | 86a: 82 30 cpi r24, 0x02 ; 2 1617 | 86c: 21 f4 brne .+8 ; 0x876 1618 | { 1619 | servo_3(param_1); 1620 | 86e: 80 91 10 02 lds r24, 0x0210 1621 | 872: 0e 94 5b 02 call 0x4b6 ; 0x4b6 1622 | } 1623 | 1624 | if (function_type == 0x03 ) 1625 | 876: 80 91 13 02 lds r24, 0x0213 1626 | 87a: 83 30 cpi r24, 0x03 ; 3 1627 | 87c: 11 f4 brne .+4 ; 0x882 1628 | { 1629 | servo_1_free(); 1630 | 87e: 0e 94 74 02 call 0x4e8 ; 0x4e8 1631 | } 1632 | 1633 | if (function_type == 0x04 ) 1634 | 882: 80 91 13 02 lds r24, 0x0213 1635 | 886: 84 30 cpi r24, 0x04 ; 4 1636 | 888: 11 f4 brne .+4 ; 0x88e 1637 | { 1638 | servo_2_free(); 1639 | 88a: 0e 94 7b 02 call 0x4f6 ; 0x4f6 1640 | } 1641 | 1642 | if (function_type == 0x05 ) 1643 | 88e: 80 91 13 02 lds r24, 0x0213 1644 | 892: 85 30 cpi r24, 0x05 ; 5 1645 | 894: 11 f4 brne .+4 ; 0x89a 1646 | { 1647 | servo_3_free(); 1648 | 896: 0e 94 82 02 call 0x504 ; 0x504 1649 | } 1650 | } 1651 | if (device_id == 0x05) 1652 | 89a: 80 91 15 02 lds r24, 0x0215 1653 | 89e: 85 30 cpi r24, 0x05 ; 5 1654 | 8a0: 81 f4 brne .+32 ; 0x8c2 1655 | { 1656 | if (function_type == 0x00) 1657 | 8a2: 80 91 13 02 lds r24, 0x0213 1658 | 8a6: 88 23 and r24, r24 1659 | 8a8: 21 f4 brne .+8 ; 0x8b2 1660 | { 1661 | LED_bargraph_on(param_1); 1662 | 8aa: 80 91 10 02 lds r24, 0x0210 1663 | 8ae: 0e 94 89 02 call 0x512 ; 0x512 1664 | } 1665 | 1666 | if (function_type == 0x01) 1667 | 8b2: 80 91 13 02 lds r24, 0x0213 1668 | 8b6: 81 30 cpi r24, 0x01 ; 1 1669 | 8b8: 21 f4 brne .+8 ; 0x8c2 1670 | { 1671 | LED_bargraph_off(param_1); 1672 | 8ba: 80 91 10 02 lds r24, 0x0210 1673 | 8be: 0e 94 8f 02 call 0x51e ; 0x51e 1674 | 8c2: 08 95 ret 1675 | 1676 | 000008c4 : 1677 | } 1678 | } 1679 | 1680 | void decode_data(void) 1681 | { 1682 | while (data_copied == 1) 1683 | 8c4: 80 91 16 02 lds r24, 0x0216 1684 | 8c8: 81 30 cpi r24, 0x01 ; 1 1685 | 8ca: 09 f0 breq .+2 ; 0x8ce 1686 | 8cc: 46 c0 rjmp .+140 ; 0x95a 1687 | { 1688 | device_id = copy_packet_data[0]; 1689 | 8ce: 80 91 1b 02 lds r24, 0x021B 1690 | 8d2: 80 93 15 02 sts 0x0215, r24 1691 | device_type = copy_packet_data[1]; 1692 | 8d6: 80 91 1c 02 lds r24, 0x021C 1693 | 8da: 80 93 14 02 sts 0x0214, r24 1694 | function_type = copy_packet_data[2]; 1695 | 8de: 80 91 1d 02 lds r24, 0x021D 1696 | 8e2: 80 93 13 02 sts 0x0213, r24 1697 | param_count = copy_packet_data[3]; 1698 | 8e6: 80 91 1e 02 lds r24, 0x021E 1699 | 8ea: 80 93 12 02 sts 0x0212, r24 1700 | param_count_upper_nibbel = param_count & 0x10; 1701 | 8ee: 98 2f mov r25, r24 1702 | 8f0: 90 71 andi r25, 0x10 ; 16 1703 | 8f2: 90 93 52 02 sts 0x0252, r25 1704 | param_count_lower_nibbel = (unsigned char)(param_count & 0x0F); 1705 | 8f6: 98 2f mov r25, r24 1706 | 8f8: 9f 70 andi r25, 0x0F ; 15 1707 | 8fa: 90 93 54 02 sts 0x0254, r25 1708 | 1709 | if ((param_count & 0x10) == 0x10) 1710 | 8fe: 84 ff sbrs r24, 4 1711 | 900: 11 c0 rjmp .+34 ; 0x924 1712 | { 1713 | temp_1 = copy_packet_data[4]; 1714 | 902: 20 91 1f 02 lds r18, 0x021F 1715 | 906: 20 93 55 02 sts 0x0255, r18 1716 | temp_2 = copy_packet_data[5]; 1717 | 90a: 30 91 20 02 lds r19, 0x0220 1718 | 90e: 30 93 51 02 sts 0x0251, r19 1719 | 1720 | param_1 = 256*temp_2 + temp_1; 1721 | 912: 93 2f mov r25, r19 1722 | 914: 80 e0 ldi r24, 0x00 ; 0 1723 | 916: 82 0f add r24, r18 1724 | 918: 91 1d adc r25, r1 1725 | 91a: 90 93 11 02 sts 0x0211, r25 1726 | 91e: 80 93 10 02 sts 0x0210, r24 1727 | 922: 18 c0 rjmp .+48 ; 0x954 1728 | } 1729 | 1730 | else 1731 | { 1732 | switch (param_count_lower_nibbel) 1733 | 924: 91 30 cpi r25, 0x01 ; 1 1734 | 926: 19 f0 breq .+6 ; 0x92e 1735 | 928: 92 30 cpi r25, 0x02 ; 2 1736 | 92a: a1 f4 brne .+40 ; 0x954 1737 | 92c: 07 c0 rjmp .+14 ; 0x93c 1738 | { 1739 | case 1: 1740 | param_1 = copy_packet_data[4]; 1741 | 92e: 80 91 1f 02 lds r24, 0x021F 1742 | 932: 80 93 10 02 sts 0x0210, r24 1743 | 936: 10 92 11 02 sts 0x0211, r1 1744 | break; 1745 | 93a: 0c c0 rjmp .+24 ; 0x954 1746 | case 2: 1747 | param_1 = copy_packet_data[4]; 1748 | 93c: 80 91 1f 02 lds r24, 0x021F 1749 | 940: 80 93 10 02 sts 0x0210, r24 1750 | 944: 10 92 11 02 sts 0x0211, r1 1751 | param_2 = copy_packet_data[5]; 1752 | 948: 80 91 20 02 lds r24, 0x0220 1753 | 94c: 80 93 0e 02 sts 0x020E, r24 1754 | 950: 10 92 0f 02 sts 0x020F, r1 1755 | break; 1756 | default: 1757 | break; 1758 | } 1759 | } 1760 | data_copied = 0; 1761 | 954: 10 92 16 02 sts 0x0216, r1 1762 | 958: 02 c0 rjmp .+4 ; 0x95e 1763 | // UDR2 = 'D'; 1764 | } 1765 | 1766 | if ((data_copied == 0) && (device_type == 0x00)) // input devices such as sensors, which will send back data 1767 | 95a: 88 23 and r24, r24 1768 | 95c: 61 f4 brne .+24 ; 0x976 1769 | 95e: 80 91 14 02 lds r24, 0x0214 1770 | 962: 88 23 and r24, r24 1771 | 964: 31 f4 brne .+12 ; 0x972 1772 | { 1773 | send_sensor_data(); 1774 | 966: 0e 94 1a 03 call 0x634 ; 0x634 1775 | 96a: 08 95 ret 1776 | } 1777 | 1778 | else if ((data_copied == 0) && (device_type == 0x01)) // output devices such as buzzer, motors 1779 | { 1780 | // UDR2 = 'A'; 1781 | actuate_devices(); 1782 | 96c: 0e 94 c7 03 call 0x78e ; 0x78e 1783 | 970: 08 95 ret 1784 | if ((data_copied == 0) && (device_type == 0x00)) // input devices such as sensors, which will send back data 1785 | { 1786 | send_sensor_data(); 1787 | } 1788 | 1789 | else if ((data_copied == 0) && (device_type == 0x01)) // output devices such as buzzer, motors 1790 | 972: 81 30 cpi r24, 0x01 ; 1 1791 | 974: d9 f3 breq .-10 ; 0x96c 1792 | 976: 08 95 ret 1793 | 1794 | 00000978 : 1795 | } 1796 | } 1797 | 1798 | void copy_data_packet() 1799 | { 1800 | if (data_packet_received == 1) 1801 | 978: 80 91 17 02 lds r24, 0x0217 1802 | 97c: 81 30 cpi r24, 0x01 ; 1 1803 | 97e: 61 f4 brne .+24 ; 0x998 1804 | //{ 1805 | //copy_packet_data[j] = uart_data_buff[j]; 1806 | ////UDR2 = copy_packet_data[j]; 1807 | //uart_data_buff[j] = 0; 1808 | //} 1809 | i=0; 1810 | 980: 10 92 19 02 sts 0x0219, r1 1811 | j=0; 1812 | 984: 10 92 18 02 sts 0x0218, r1 1813 | data_packet_received = 0; 1814 | 988: 10 92 17 02 sts 0x0217, r1 1815 | end_char_rec = 0; 1816 | 98c: 10 92 1a 02 sts 0x021A, r1 1817 | data_copied = 1; 1818 | 990: 80 93 16 02 sts 0x0216, r24 1819 | 1820 | // UDR2 = data_copied; 1821 | decode_data(); 1822 | 994: 0e 94 62 04 call 0x8c4 ; 0x8c4 1823 | 998: 08 95 ret 1824 | 1825 | 0000099a
: 1826 | } 1827 | 1828 | //Main Function 1829 | int main(void) 1830 | { 1831 | init_devices(); 1832 | 99a: 0e 94 9d 01 call 0x33a ; 0x33a 1833 | servo_2(0); 1834 | 99e: 80 e0 ldi r24, 0x00 ; 0 1835 | 9a0: 0e 94 42 02 call 0x484 ; 0x484 1836 | servo_3(0); 1837 | 9a4: 80 e0 ldi r24, 0x00 ; 0 1838 | 9a6: 0e 94 5b 02 call 0x4b6 ; 0x4b6 1839 | while(1) 1840 | { 1841 | copy_data_packet(); 1842 | 9aa: 0e 94 bc 04 call 0x978 ; 0x978 1843 | 9ae: fd cf rjmp .-6 ; 0x9aa 1844 | 1845 | 000009b0 <__subsf3>: 1846 | 9b0: 50 58 subi r21, 0x80 ; 128 1847 | 1848 | 000009b2 <__addsf3>: 1849 | 9b2: bb 27 eor r27, r27 1850 | 9b4: aa 27 eor r26, r26 1851 | 9b6: 0e d0 rcall .+28 ; 0x9d4 <__addsf3x> 1852 | 9b8: 48 c1 rjmp .+656 ; 0xc4a <__fp_round> 1853 | 9ba: 39 d1 rcall .+626 ; 0xc2e <__fp_pscA> 1854 | 9bc: 30 f0 brcs .+12 ; 0x9ca <__addsf3+0x18> 1855 | 9be: 3e d1 rcall .+636 ; 0xc3c <__fp_pscB> 1856 | 9c0: 20 f0 brcs .+8 ; 0x9ca <__addsf3+0x18> 1857 | 9c2: 31 f4 brne .+12 ; 0x9d0 <__addsf3+0x1e> 1858 | 9c4: 9f 3f cpi r25, 0xFF ; 255 1859 | 9c6: 11 f4 brne .+4 ; 0x9cc <__addsf3+0x1a> 1860 | 9c8: 1e f4 brtc .+6 ; 0x9d0 <__addsf3+0x1e> 1861 | 9ca: 2e c1 rjmp .+604 ; 0xc28 <__fp_nan> 1862 | 9cc: 0e f4 brtc .+2 ; 0x9d0 <__addsf3+0x1e> 1863 | 9ce: e0 95 com r30 1864 | 9d0: e7 fb bst r30, 7 1865 | 9d2: 24 c1 rjmp .+584 ; 0xc1c <__fp_inf> 1866 | 1867 | 000009d4 <__addsf3x>: 1868 | 9d4: e9 2f mov r30, r25 1869 | 9d6: 4a d1 rcall .+660 ; 0xc6c <__fp_split3> 1870 | 9d8: 80 f3 brcs .-32 ; 0x9ba <__addsf3+0x8> 1871 | 9da: ba 17 cp r27, r26 1872 | 9dc: 62 07 cpc r22, r18 1873 | 9de: 73 07 cpc r23, r19 1874 | 9e0: 84 07 cpc r24, r20 1875 | 9e2: 95 07 cpc r25, r21 1876 | 9e4: 18 f0 brcs .+6 ; 0x9ec <__addsf3x+0x18> 1877 | 9e6: 71 f4 brne .+28 ; 0xa04 <__addsf3x+0x30> 1878 | 9e8: 9e f5 brtc .+102 ; 0xa50 <__addsf3x+0x7c> 1879 | 9ea: 62 c1 rjmp .+708 ; 0xcb0 <__fp_zero> 1880 | 9ec: 0e f4 brtc .+2 ; 0x9f0 <__addsf3x+0x1c> 1881 | 9ee: e0 95 com r30 1882 | 9f0: 0b 2e mov r0, r27 1883 | 9f2: ba 2f mov r27, r26 1884 | 9f4: a0 2d mov r26, r0 1885 | 9f6: 0b 01 movw r0, r22 1886 | 9f8: b9 01 movw r22, r18 1887 | 9fa: 90 01 movw r18, r0 1888 | 9fc: 0c 01 movw r0, r24 1889 | 9fe: ca 01 movw r24, r20 1890 | a00: a0 01 movw r20, r0 1891 | a02: 11 24 eor r1, r1 1892 | a04: ff 27 eor r31, r31 1893 | a06: 59 1b sub r21, r25 1894 | a08: 99 f0 breq .+38 ; 0xa30 <__addsf3x+0x5c> 1895 | a0a: 59 3f cpi r21, 0xF9 ; 249 1896 | a0c: 50 f4 brcc .+20 ; 0xa22 <__addsf3x+0x4e> 1897 | a0e: 50 3e cpi r21, 0xE0 ; 224 1898 | a10: 68 f1 brcs .+90 ; 0xa6c <__addsf3x+0x98> 1899 | a12: 1a 16 cp r1, r26 1900 | a14: f0 40 sbci r31, 0x00 ; 0 1901 | a16: a2 2f mov r26, r18 1902 | a18: 23 2f mov r18, r19 1903 | a1a: 34 2f mov r19, r20 1904 | a1c: 44 27 eor r20, r20 1905 | a1e: 58 5f subi r21, 0xF8 ; 248 1906 | a20: f3 cf rjmp .-26 ; 0xa08 <__addsf3x+0x34> 1907 | a22: 46 95 lsr r20 1908 | a24: 37 95 ror r19 1909 | a26: 27 95 ror r18 1910 | a28: a7 95 ror r26 1911 | a2a: f0 40 sbci r31, 0x00 ; 0 1912 | a2c: 53 95 inc r21 1913 | a2e: c9 f7 brne .-14 ; 0xa22 <__addsf3x+0x4e> 1914 | a30: 7e f4 brtc .+30 ; 0xa50 <__addsf3x+0x7c> 1915 | a32: 1f 16 cp r1, r31 1916 | a34: ba 0b sbc r27, r26 1917 | a36: 62 0b sbc r22, r18 1918 | a38: 73 0b sbc r23, r19 1919 | a3a: 84 0b sbc r24, r20 1920 | a3c: ba f0 brmi .+46 ; 0xa6c <__addsf3x+0x98> 1921 | a3e: 91 50 subi r25, 0x01 ; 1 1922 | a40: a1 f0 breq .+40 ; 0xa6a <__addsf3x+0x96> 1923 | a42: ff 0f add r31, r31 1924 | a44: bb 1f adc r27, r27 1925 | a46: 66 1f adc r22, r22 1926 | a48: 77 1f adc r23, r23 1927 | a4a: 88 1f adc r24, r24 1928 | a4c: c2 f7 brpl .-16 ; 0xa3e <__addsf3x+0x6a> 1929 | a4e: 0e c0 rjmp .+28 ; 0xa6c <__addsf3x+0x98> 1930 | a50: ba 0f add r27, r26 1931 | a52: 62 1f adc r22, r18 1932 | a54: 73 1f adc r23, r19 1933 | a56: 84 1f adc r24, r20 1934 | a58: 48 f4 brcc .+18 ; 0xa6c <__addsf3x+0x98> 1935 | a5a: 87 95 ror r24 1936 | a5c: 77 95 ror r23 1937 | a5e: 67 95 ror r22 1938 | a60: b7 95 ror r27 1939 | a62: f7 95 ror r31 1940 | a64: 9e 3f cpi r25, 0xFE ; 254 1941 | a66: 08 f0 brcs .+2 ; 0xa6a <__addsf3x+0x96> 1942 | a68: b3 cf rjmp .-154 ; 0x9d0 <__addsf3+0x1e> 1943 | a6a: 93 95 inc r25 1944 | a6c: 88 0f add r24, r24 1945 | a6e: 08 f0 brcs .+2 ; 0xa72 <__addsf3x+0x9e> 1946 | a70: 99 27 eor r25, r25 1947 | a72: ee 0f add r30, r30 1948 | a74: 97 95 ror r25 1949 | a76: 87 95 ror r24 1950 | a78: 08 95 ret 1951 | 1952 | 00000a7a <__divsf3>: 1953 | a7a: 0c d0 rcall .+24 ; 0xa94 <__divsf3x> 1954 | a7c: e6 c0 rjmp .+460 ; 0xc4a <__fp_round> 1955 | a7e: de d0 rcall .+444 ; 0xc3c <__fp_pscB> 1956 | a80: 40 f0 brcs .+16 ; 0xa92 <__divsf3+0x18> 1957 | a82: d5 d0 rcall .+426 ; 0xc2e <__fp_pscA> 1958 | a84: 30 f0 brcs .+12 ; 0xa92 <__divsf3+0x18> 1959 | a86: 21 f4 brne .+8 ; 0xa90 <__divsf3+0x16> 1960 | a88: 5f 3f cpi r21, 0xFF ; 255 1961 | a8a: 19 f0 breq .+6 ; 0xa92 <__divsf3+0x18> 1962 | a8c: c7 c0 rjmp .+398 ; 0xc1c <__fp_inf> 1963 | a8e: 51 11 cpse r21, r1 1964 | a90: 10 c1 rjmp .+544 ; 0xcb2 <__fp_szero> 1965 | a92: ca c0 rjmp .+404 ; 0xc28 <__fp_nan> 1966 | 1967 | 00000a94 <__divsf3x>: 1968 | a94: eb d0 rcall .+470 ; 0xc6c <__fp_split3> 1969 | a96: 98 f3 brcs .-26 ; 0xa7e <__divsf3+0x4> 1970 | 1971 | 00000a98 <__divsf3_pse>: 1972 | a98: 99 23 and r25, r25 1973 | a9a: c9 f3 breq .-14 ; 0xa8e <__divsf3+0x14> 1974 | a9c: 55 23 and r21, r21 1975 | a9e: b1 f3 breq .-20 ; 0xa8c <__divsf3+0x12> 1976 | aa0: 95 1b sub r25, r21 1977 | aa2: 55 0b sbc r21, r21 1978 | aa4: bb 27 eor r27, r27 1979 | aa6: aa 27 eor r26, r26 1980 | aa8: 62 17 cp r22, r18 1981 | aaa: 73 07 cpc r23, r19 1982 | aac: 84 07 cpc r24, r20 1983 | aae: 38 f0 brcs .+14 ; 0xabe <__divsf3_pse+0x26> 1984 | ab0: 9f 5f subi r25, 0xFF ; 255 1985 | ab2: 5f 4f sbci r21, 0xFF ; 255 1986 | ab4: 22 0f add r18, r18 1987 | ab6: 33 1f adc r19, r19 1988 | ab8: 44 1f adc r20, r20 1989 | aba: aa 1f adc r26, r26 1990 | abc: a9 f3 breq .-22 ; 0xaa8 <__divsf3_pse+0x10> 1991 | abe: 33 d0 rcall .+102 ; 0xb26 <__divsf3_pse+0x8e> 1992 | ac0: 0e 2e mov r0, r30 1993 | ac2: 3a f0 brmi .+14 ; 0xad2 <__divsf3_pse+0x3a> 1994 | ac4: e0 e8 ldi r30, 0x80 ; 128 1995 | ac6: 30 d0 rcall .+96 ; 0xb28 <__divsf3_pse+0x90> 1996 | ac8: 91 50 subi r25, 0x01 ; 1 1997 | aca: 50 40 sbci r21, 0x00 ; 0 1998 | acc: e6 95 lsr r30 1999 | ace: 00 1c adc r0, r0 2000 | ad0: ca f7 brpl .-14 ; 0xac4 <__divsf3_pse+0x2c> 2001 | ad2: 29 d0 rcall .+82 ; 0xb26 <__divsf3_pse+0x8e> 2002 | ad4: fe 2f mov r31, r30 2003 | ad6: 27 d0 rcall .+78 ; 0xb26 <__divsf3_pse+0x8e> 2004 | ad8: 66 0f add r22, r22 2005 | ada: 77 1f adc r23, r23 2006 | adc: 88 1f adc r24, r24 2007 | ade: bb 1f adc r27, r27 2008 | ae0: 26 17 cp r18, r22 2009 | ae2: 37 07 cpc r19, r23 2010 | ae4: 48 07 cpc r20, r24 2011 | ae6: ab 07 cpc r26, r27 2012 | ae8: b0 e8 ldi r27, 0x80 ; 128 2013 | aea: 09 f0 breq .+2 ; 0xaee <__divsf3_pse+0x56> 2014 | aec: bb 0b sbc r27, r27 2015 | aee: 80 2d mov r24, r0 2016 | af0: bf 01 movw r22, r30 2017 | af2: ff 27 eor r31, r31 2018 | af4: 93 58 subi r25, 0x83 ; 131 2019 | af6: 5f 4f sbci r21, 0xFF ; 255 2020 | af8: 2a f0 brmi .+10 ; 0xb04 <__divsf3_pse+0x6c> 2021 | afa: 9e 3f cpi r25, 0xFE ; 254 2022 | afc: 51 05 cpc r21, r1 2023 | afe: 68 f0 brcs .+26 ; 0xb1a <__divsf3_pse+0x82> 2024 | b00: 8d c0 rjmp .+282 ; 0xc1c <__fp_inf> 2025 | b02: d7 c0 rjmp .+430 ; 0xcb2 <__fp_szero> 2026 | b04: 5f 3f cpi r21, 0xFF ; 255 2027 | b06: ec f3 brlt .-6 ; 0xb02 <__divsf3_pse+0x6a> 2028 | b08: 98 3e cpi r25, 0xE8 ; 232 2029 | b0a: dc f3 brlt .-10 ; 0xb02 <__divsf3_pse+0x6a> 2030 | b0c: 86 95 lsr r24 2031 | b0e: 77 95 ror r23 2032 | b10: 67 95 ror r22 2033 | b12: b7 95 ror r27 2034 | b14: f7 95 ror r31 2035 | b16: 9f 5f subi r25, 0xFF ; 255 2036 | b18: c9 f7 brne .-14 ; 0xb0c <__divsf3_pse+0x74> 2037 | b1a: 88 0f add r24, r24 2038 | b1c: 91 1d adc r25, r1 2039 | b1e: 96 95 lsr r25 2040 | b20: 87 95 ror r24 2041 | b22: 97 f9 bld r25, 7 2042 | b24: 08 95 ret 2043 | b26: e1 e0 ldi r30, 0x01 ; 1 2044 | b28: 66 0f add r22, r22 2045 | b2a: 77 1f adc r23, r23 2046 | b2c: 88 1f adc r24, r24 2047 | b2e: bb 1f adc r27, r27 2048 | b30: 62 17 cp r22, r18 2049 | b32: 73 07 cpc r23, r19 2050 | b34: 84 07 cpc r24, r20 2051 | b36: ba 07 cpc r27, r26 2052 | b38: 20 f0 brcs .+8 ; 0xb42 <__divsf3_pse+0xaa> 2053 | b3a: 62 1b sub r22, r18 2054 | b3c: 73 0b sbc r23, r19 2055 | b3e: 84 0b sbc r24, r20 2056 | b40: ba 0b sbc r27, r26 2057 | b42: ee 1f adc r30, r30 2058 | b44: 88 f7 brcc .-30 ; 0xb28 <__divsf3_pse+0x90> 2059 | b46: e0 95 com r30 2060 | b48: 08 95 ret 2061 | 2062 | 00000b4a <__fixunssfsi>: 2063 | b4a: 98 d0 rcall .+304 ; 0xc7c <__fp_splitA> 2064 | b4c: 88 f0 brcs .+34 ; 0xb70 <__fixunssfsi+0x26> 2065 | b4e: 9f 57 subi r25, 0x7F ; 127 2066 | b50: 90 f0 brcs .+36 ; 0xb76 <__fixunssfsi+0x2c> 2067 | b52: b9 2f mov r27, r25 2068 | b54: 99 27 eor r25, r25 2069 | b56: b7 51 subi r27, 0x17 ; 23 2070 | b58: a0 f0 brcs .+40 ; 0xb82 <__fixunssfsi+0x38> 2071 | b5a: d1 f0 breq .+52 ; 0xb90 <__fixunssfsi+0x46> 2072 | b5c: 66 0f add r22, r22 2073 | b5e: 77 1f adc r23, r23 2074 | b60: 88 1f adc r24, r24 2075 | b62: 99 1f adc r25, r25 2076 | b64: 1a f0 brmi .+6 ; 0xb6c <__fixunssfsi+0x22> 2077 | b66: ba 95 dec r27 2078 | b68: c9 f7 brne .-14 ; 0xb5c <__fixunssfsi+0x12> 2079 | b6a: 12 c0 rjmp .+36 ; 0xb90 <__fixunssfsi+0x46> 2080 | b6c: b1 30 cpi r27, 0x01 ; 1 2081 | b6e: 81 f0 breq .+32 ; 0xb90 <__fixunssfsi+0x46> 2082 | b70: 9f d0 rcall .+318 ; 0xcb0 <__fp_zero> 2083 | b72: b1 e0 ldi r27, 0x01 ; 1 2084 | b74: 08 95 ret 2085 | b76: 9c c0 rjmp .+312 ; 0xcb0 <__fp_zero> 2086 | b78: 67 2f mov r22, r23 2087 | b7a: 78 2f mov r23, r24 2088 | b7c: 88 27 eor r24, r24 2089 | b7e: b8 5f subi r27, 0xF8 ; 248 2090 | b80: 39 f0 breq .+14 ; 0xb90 <__fixunssfsi+0x46> 2091 | b82: b9 3f cpi r27, 0xF9 ; 249 2092 | b84: cc f3 brlt .-14 ; 0xb78 <__fixunssfsi+0x2e> 2093 | b86: 86 95 lsr r24 2094 | b88: 77 95 ror r23 2095 | b8a: 67 95 ror r22 2096 | b8c: b3 95 inc r27 2097 | b8e: d9 f7 brne .-10 ; 0xb86 <__fixunssfsi+0x3c> 2098 | b90: 3e f4 brtc .+14 ; 0xba0 <__fixunssfsi+0x56> 2099 | b92: 90 95 com r25 2100 | b94: 80 95 com r24 2101 | b96: 70 95 com r23 2102 | b98: 61 95 neg r22 2103 | b9a: 7f 4f sbci r23, 0xFF ; 255 2104 | b9c: 8f 4f sbci r24, 0xFF ; 255 2105 | b9e: 9f 4f sbci r25, 0xFF ; 255 2106 | ba0: 08 95 ret 2107 | 2108 | 00000ba2 <__floatunsisf>: 2109 | ba2: e8 94 clt 2110 | ba4: 09 c0 rjmp .+18 ; 0xbb8 <__floatsisf+0x12> 2111 | 2112 | 00000ba6 <__floatsisf>: 2113 | ba6: 97 fb bst r25, 7 2114 | ba8: 3e f4 brtc .+14 ; 0xbb8 <__floatsisf+0x12> 2115 | baa: 90 95 com r25 2116 | bac: 80 95 com r24 2117 | bae: 70 95 com r23 2118 | bb0: 61 95 neg r22 2119 | bb2: 7f 4f sbci r23, 0xFF ; 255 2120 | bb4: 8f 4f sbci r24, 0xFF ; 255 2121 | bb6: 9f 4f sbci r25, 0xFF ; 255 2122 | bb8: 99 23 and r25, r25 2123 | bba: a9 f0 breq .+42 ; 0xbe6 <__floatsisf+0x40> 2124 | bbc: f9 2f mov r31, r25 2125 | bbe: 96 e9 ldi r25, 0x96 ; 150 2126 | bc0: bb 27 eor r27, r27 2127 | bc2: 93 95 inc r25 2128 | bc4: f6 95 lsr r31 2129 | bc6: 87 95 ror r24 2130 | bc8: 77 95 ror r23 2131 | bca: 67 95 ror r22 2132 | bcc: b7 95 ror r27 2133 | bce: f1 11 cpse r31, r1 2134 | bd0: f8 cf rjmp .-16 ; 0xbc2 <__floatsisf+0x1c> 2135 | bd2: fa f4 brpl .+62 ; 0xc12 <__floatsisf+0x6c> 2136 | bd4: bb 0f add r27, r27 2137 | bd6: 11 f4 brne .+4 ; 0xbdc <__floatsisf+0x36> 2138 | bd8: 60 ff sbrs r22, 0 2139 | bda: 1b c0 rjmp .+54 ; 0xc12 <__floatsisf+0x6c> 2140 | bdc: 6f 5f subi r22, 0xFF ; 255 2141 | bde: 7f 4f sbci r23, 0xFF ; 255 2142 | be0: 8f 4f sbci r24, 0xFF ; 255 2143 | be2: 9f 4f sbci r25, 0xFF ; 255 2144 | be4: 16 c0 rjmp .+44 ; 0xc12 <__floatsisf+0x6c> 2145 | be6: 88 23 and r24, r24 2146 | be8: 11 f0 breq .+4 ; 0xbee <__floatsisf+0x48> 2147 | bea: 96 e9 ldi r25, 0x96 ; 150 2148 | bec: 11 c0 rjmp .+34 ; 0xc10 <__floatsisf+0x6a> 2149 | bee: 77 23 and r23, r23 2150 | bf0: 21 f0 breq .+8 ; 0xbfa <__floatsisf+0x54> 2151 | bf2: 9e e8 ldi r25, 0x8E ; 142 2152 | bf4: 87 2f mov r24, r23 2153 | bf6: 76 2f mov r23, r22 2154 | bf8: 05 c0 rjmp .+10 ; 0xc04 <__floatsisf+0x5e> 2155 | bfa: 66 23 and r22, r22 2156 | bfc: 71 f0 breq .+28 ; 0xc1a <__floatsisf+0x74> 2157 | bfe: 96 e8 ldi r25, 0x86 ; 134 2158 | c00: 86 2f mov r24, r22 2159 | c02: 70 e0 ldi r23, 0x00 ; 0 2160 | c04: 60 e0 ldi r22, 0x00 ; 0 2161 | c06: 2a f0 brmi .+10 ; 0xc12 <__floatsisf+0x6c> 2162 | c08: 9a 95 dec r25 2163 | c0a: 66 0f add r22, r22 2164 | c0c: 77 1f adc r23, r23 2165 | c0e: 88 1f adc r24, r24 2166 | c10: da f7 brpl .-10 ; 0xc08 <__floatsisf+0x62> 2167 | c12: 88 0f add r24, r24 2168 | c14: 96 95 lsr r25 2169 | c16: 87 95 ror r24 2170 | c18: 97 f9 bld r25, 7 2171 | c1a: 08 95 ret 2172 | 2173 | 00000c1c <__fp_inf>: 2174 | c1c: 97 f9 bld r25, 7 2175 | c1e: 9f 67 ori r25, 0x7F ; 127 2176 | c20: 80 e8 ldi r24, 0x80 ; 128 2177 | c22: 70 e0 ldi r23, 0x00 ; 0 2178 | c24: 60 e0 ldi r22, 0x00 ; 0 2179 | c26: 08 95 ret 2180 | 2181 | 00000c28 <__fp_nan>: 2182 | c28: 9f ef ldi r25, 0xFF ; 255 2183 | c2a: 80 ec ldi r24, 0xC0 ; 192 2184 | c2c: 08 95 ret 2185 | 2186 | 00000c2e <__fp_pscA>: 2187 | c2e: 00 24 eor r0, r0 2188 | c30: 0a 94 dec r0 2189 | c32: 16 16 cp r1, r22 2190 | c34: 17 06 cpc r1, r23 2191 | c36: 18 06 cpc r1, r24 2192 | c38: 09 06 cpc r0, r25 2193 | c3a: 08 95 ret 2194 | 2195 | 00000c3c <__fp_pscB>: 2196 | c3c: 00 24 eor r0, r0 2197 | c3e: 0a 94 dec r0 2198 | c40: 12 16 cp r1, r18 2199 | c42: 13 06 cpc r1, r19 2200 | c44: 14 06 cpc r1, r20 2201 | c46: 05 06 cpc r0, r21 2202 | c48: 08 95 ret 2203 | 2204 | 00000c4a <__fp_round>: 2205 | c4a: 09 2e mov r0, r25 2206 | c4c: 03 94 inc r0 2207 | c4e: 00 0c add r0, r0 2208 | c50: 11 f4 brne .+4 ; 0xc56 <__fp_round+0xc> 2209 | c52: 88 23 and r24, r24 2210 | c54: 52 f0 brmi .+20 ; 0xc6a <__fp_round+0x20> 2211 | c56: bb 0f add r27, r27 2212 | c58: 40 f4 brcc .+16 ; 0xc6a <__fp_round+0x20> 2213 | c5a: bf 2b or r27, r31 2214 | c5c: 11 f4 brne .+4 ; 0xc62 <__fp_round+0x18> 2215 | c5e: 60 ff sbrs r22, 0 2216 | c60: 04 c0 rjmp .+8 ; 0xc6a <__fp_round+0x20> 2217 | c62: 6f 5f subi r22, 0xFF ; 255 2218 | c64: 7f 4f sbci r23, 0xFF ; 255 2219 | c66: 8f 4f sbci r24, 0xFF ; 255 2220 | c68: 9f 4f sbci r25, 0xFF ; 255 2221 | c6a: 08 95 ret 2222 | 2223 | 00000c6c <__fp_split3>: 2224 | c6c: 57 fd sbrc r21, 7 2225 | c6e: 90 58 subi r25, 0x80 ; 128 2226 | c70: 44 0f add r20, r20 2227 | c72: 55 1f adc r21, r21 2228 | c74: 59 f0 breq .+22 ; 0xc8c <__fp_splitA+0x10> 2229 | c76: 5f 3f cpi r21, 0xFF ; 255 2230 | c78: 71 f0 breq .+28 ; 0xc96 <__fp_splitA+0x1a> 2231 | c7a: 47 95 ror r20 2232 | 2233 | 00000c7c <__fp_splitA>: 2234 | c7c: 88 0f add r24, r24 2235 | c7e: 97 fb bst r25, 7 2236 | c80: 99 1f adc r25, r25 2237 | c82: 61 f0 breq .+24 ; 0xc9c <__fp_splitA+0x20> 2238 | c84: 9f 3f cpi r25, 0xFF ; 255 2239 | c86: 79 f0 breq .+30 ; 0xca6 <__fp_splitA+0x2a> 2240 | c88: 87 95 ror r24 2241 | c8a: 08 95 ret 2242 | c8c: 12 16 cp r1, r18 2243 | c8e: 13 06 cpc r1, r19 2244 | c90: 14 06 cpc r1, r20 2245 | c92: 55 1f adc r21, r21 2246 | c94: f2 cf rjmp .-28 ; 0xc7a <__fp_split3+0xe> 2247 | c96: 46 95 lsr r20 2248 | c98: f1 df rcall .-30 ; 0xc7c <__fp_splitA> 2249 | c9a: 08 c0 rjmp .+16 ; 0xcac <__fp_splitA+0x30> 2250 | c9c: 16 16 cp r1, r22 2251 | c9e: 17 06 cpc r1, r23 2252 | ca0: 18 06 cpc r1, r24 2253 | ca2: 99 1f adc r25, r25 2254 | ca4: f1 cf rjmp .-30 ; 0xc88 <__fp_splitA+0xc> 2255 | ca6: 86 95 lsr r24 2256 | ca8: 71 05 cpc r23, r1 2257 | caa: 61 05 cpc r22, r1 2258 | cac: 08 94 sec 2259 | cae: 08 95 ret 2260 | 2261 | 00000cb0 <__fp_zero>: 2262 | cb0: e8 94 clt 2263 | 2264 | 00000cb2 <__fp_szero>: 2265 | cb2: bb 27 eor r27, r27 2266 | cb4: 66 27 eor r22, r22 2267 | cb6: 77 27 eor r23, r23 2268 | cb8: cb 01 movw r24, r22 2269 | cba: 97 f9 bld r25, 7 2270 | cbc: 08 95 ret 2271 | 2272 | 00000cbe <_exit>: 2273 | cbe: f8 94 cli 2274 | 2275 | 00000cc0 <__stop_program>: 2276 | cc0: ff cf rjmp .-2 ; 0xcc0 <__stop_program> 2277 | --------------------------------------------------------------------------------