├── .gdbinit ├── .gitignore ├── README.md ├── arm-elf-target.atp ├── docs ├── DC power jack.pdf ├── F4 Disco schematics.pdf ├── nxt analog sensors.pro ├── nxt digital sensors.pro ├── rc_car system architecture.pdf ├── rc_car system architecture.pptx ├── stm32f4disco wiring diagrams.pdf └── stm32f4disco wiring diagrams.pptx ├── gnat.adc ├── rc_car.gpr └── src ├── ble ├── adafruit-ble_msg_utils.adb ├── adafruit-ble_msg_utils.ads ├── adafruit-bluefruit_le_friend.adb ├── adafruit-bluefruit_le_friend.ads ├── bluefruit_le_friend_usart.ads ├── serial_io-interrupt_driven.adb ├── serial_io-interrupt_driven.ads ├── serial_io.adb └── serial_io.ads ├── collision_detection.adb ├── collision_detection.ads ├── engine_control.adb ├── engine_control.ads ├── global_initialization.adb ├── global_initialization.ads ├── hardware_configuration_ble.ads ├── hardware_configuration_ir.ads ├── rc_car.adb ├── remote_control.ads ├── remote_control_adafruit_ble_app.adb ├── remote_control_ir_pf8879.adb ├── steering_control.adb ├── steering_control.ads ├── system_configuration.ads ├── vehicle.adb └── vehicle.ads /.gdbinit: -------------------------------------------------------------------------------- 1 | # This command file will cause a Cortex-M3 or -M4 board to automatically 2 | # reset immediately after a GDB "load" command executes. Note that GPS 3 | # issues that command as part of the Debug->Init menu invocation. Manual 4 | # "load" command invocations will also trigger the action. 5 | # 6 | # The reset is achieved by writing to the "Application Interrupt and Reset 7 | # Control" register located at address 0xE000ED0C. 8 | # 9 | # Both the processor and the peripherals can be reset by writing a value 10 | # of 0x05FA0004. That value will write to the SYSRESETREQ bit. If you want 11 | # to avoid resetting the peripherals, change the value to 0x05FA0001. That 12 | # value will write to the VECTRESET bit. Do *not* use a value that sets both 13 | # bits. 14 | # 15 | # In both cases, any on-board debug hardware is not reset. 16 | # 17 | # See the book "The Definitive Guide to the ARM Cortex-M3 and Cortex-M4 18 | # Processors" by Joseph Yiu, 3rd edition, pp 262-263 for further details. 19 | 20 | define hookpost-load 21 | echo Resetting the processor and peripherals...\n 22 | set *0xE000ED0C := 0x05FA0004 23 | echo Reset complete\n 24 | end -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | obj 2 | Ada_Drivers_Library 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RC_Car_Demo 2 | Demonstrating Robotics with Ada, SPARK, ARM, and Lego Sensors/Effectors 3 | 4 | --- 5 | 6 | IMPORTANT: Please note that this project exists as part of a blog entry, 7 | article, or other similar material by AdaCore. It is provided for 8 | convenient access to the software described therein. As such, it is not 9 | updated regularly and may, over time, become inconsistent with the 10 | latest versions of any tools and libraries it utilizes (for example, the 11 | Ada Drivers Library). 12 | 13 | --- 14 | 15 | This program demonstrates the use of Ada and SPARK in an embedded environment. 16 | Specifically, we have a remote-controlled car using Lego NXT motors and sensors 17 | but without the Lego NXT Brick. All the software is fully in Ada and SPARK. 18 | No Lego drivers are used whatsoever. 19 | 20 | The control program and the physical car itself are based on the HiTechnic IR 21 | RC Car, with several significant differences. Not least is the fact that no 22 | LEGO NXT brick is used, requiring a replacement for the Brick and its battery 23 | pack. 24 | 25 | Instructions for building their original car are here: 26 | 27 | http://www.hitechnic.com/models 28 | 29 | A video of their car is available on YouTube: 30 | 31 | https://www.youtube.com/watch?v=KltnZBSvLu4 32 | 33 | The computer replacing the Lego Brick is a 32-bit ARM Cortex-M4 MCU on the 34 | STM32F4 Discovery card by STMicroelectronics. An FPU is included so we use 35 | floating-point in the program. 36 | 37 | https://www.st.com/en/evaluation-tools/stm32f4discovery.html 38 | 39 | In addition, we use a third-party hardware card known as the NXT Shield to 40 | interface to the NXT motors and sonar scanner. Specifically, we use "NXT 41 | Shield Version 2" produced by TKJ Electronics. 42 | 43 | http://blog.tkjelectronics.dk/2011/10/nxt-shield-ver2/ 44 | 45 | http://shop.tkjelectronics.dk/product_info.php?products_id=29 46 | 47 | We use a battery that provides separate connections for +5 and +9 (or +12) 48 | volts. The 5V is provided via USB connector, which is precisely what the 49 | STM32F4 card requires for power. It isn't light but holds a charge for a 50 | long time. The battery is the "XTPower AE-MP-10000-External-Battery" pack. 51 | 52 | https://www.xtpower.com/ 53 | 54 | Available remote controls: 55 | 56 | * Lego Power_Functions_IR_TX_8879 57 | see https://www.lego.com/en-us/themes/power-functions/products/ir-speed-remote-control-8879 58 | 59 | * AdaFruit app on cell phone, using BLE for the connection 60 | see https://learn.adafruit.com/bluefruit-le-connect/controller 61 | 62 | The AdaFruit remote app uses a Bluetooth connection from the iPhone or 63 | Android mobile phone. The other remote use IR, so for that we use the 64 | HiTechnic IR receiver. If the AdaFruit BLE is used we must swap out the IR 65 | receiver for the Bluefruit LE UART Friend receiver. 66 | 67 | There is one package declaration for the remote control interface. Each 68 | different remote control device above has a corresponding package body 69 | implementing the single shared package spec. To have the selected remote be 70 | used in the RC_Car program you must build the program with the package body 71 | corresponding to the desired controller. This selection is accomplished via 72 | the "Remote_Control" scenario variable. 73 | 74 | See the package bodies for how to physically use the corresponding remote controls. 75 | Note that the BLE version of the package body is probably out of date because 76 | we primarily use the Lego remote. 77 | 78 | There is an AdaCore Blog entry describing the project: 79 | 80 | https://blog.adacore.com/making-an-rc-car-with-ada-and-spark 81 | 82 | Videos of our car in action: 83 | 84 | * https://youtu.be/Hngzh5zDM3E 85 | 86 | * https://youtu.be/nK9uDJ4909M (in which we can hear the ultrasonic sensor pings) 87 | -------------------------------------------------------------------------------- /arm-elf-target.atp: -------------------------------------------------------------------------------- 1 | Bits_BE 0 2 | Bits_Per_Unit 8 3 | Bits_Per_Word 32 4 | Bytes_BE 0 5 | Char_Size 8 6 | Double_Float_Alignment 0 7 | Double_Scalar_Alignment 0 8 | Double_Size 64 9 | Float_Size 32 10 | Float_Words_BE 0 11 | Int_Size 32 12 | Long_Double_Size 64 13 | Long_Long_Size 64 14 | Long_Size 32 15 | Maximum_Alignment 8 16 | Max_Unaligned_Field 64 17 | Pointer_Size 32 18 | Short_Enums 1 19 | Short_Size 16 20 | Strict_Alignment 1 21 | System_Allocator_Alignment 8 22 | Wchar_T_Size 32 23 | Words_BE 0 24 | 25 | HF 3 I 16 16 26 | float 6 I 32 32 27 | double 15 I 64 64 28 | long double 15 I 64 64 29 | -------------------------------------------------------------------------------- /docs/DC power jack.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AdaCore/RC_Car_Demo/1b6bac7ed90b3ea0d44f4c3f40b374cc7f1fddcc/docs/DC power jack.pdf -------------------------------------------------------------------------------- /docs/F4 Disco schematics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AdaCore/RC_Car_Demo/1b6bac7ed90b3ea0d44f4c3f40b374cc7f1fddcc/docs/F4 Disco schematics.pdf -------------------------------------------------------------------------------- /docs/nxt analog sensors.pro: -------------------------------------------------------------------------------- 1 | (UMLStudio "7.1" project) 2 | (repository "" 0) 3 | (notation "UML.not") 4 | (genProfile 178 "C++" ("" "" "" "" "") "" 0 ("" "" "" "" "") ("" "" "" "" "") 5 | 700 360 362 578 70 50 80 50 80 60 0 0) 6 | (codeFiles) 7 | (docFiles) 8 | (otherFiles) 9 | (revFiles "C++") 10 | (masters (master "j&!MicE(!'@]D)!" 1 3 "NXT_Analog_Sensor_DMA" "" "" "" "" "" 11 | 5 12 | (("Controller" "DMA_Controller" "" "" "" "" 2 16 13 | "h&!kbcE(!'@]D)!")) 14 | (("Initialize" "" "" 3 16 nil nil "s&!^"icE(!'@]D)!" "") 15 | ("Get_Raw_Reading" "" "" 3 16 nil nil "l&!2icE(!'@]D)!" "")) 16 | nil nil nil 10 "") 17 | (master "9(!8ncE(!'@]D)!" 1 3 "NXT_Sound_Sensor" "" "" "" "" "" 1 nil 18 | (("Initialize" "" "" 3 16 nil nil "U(!hlcE(!'@]D)!" "") 19 | ("Set_Mode" "" "" 3 16 nil nil "V(!SlcE(!'@]D)!" "") 20 | ("Current_Mode" "" "" 3 16 nil nil "W(!LlcE(!'@]D)!" "")) 21 | nil nil nil 10 "") 22 | (master "4(!KncE(!'@]D)!" 1 3 "NXT_Light_Sensor" "" "" "" "" "" 1 nil 23 | (("Initialize" "" "" 3 16 nil nil "?(!KmcE(!'@]D)!" "") 24 | ("Enable_Floodlight" "" "" 3 16 nil nil "F(!0mcE(!'@]D)!" "") 25 | ("Disable_Floodlight" "" "" 3 16 nil nil "G(!)mcE(!'@]D)!" 26 | "") 27 | ("Floodlight_Enabled" "Boolean" "" 3 16 nil nil 28 | "H(!xlcE(!'@]D)!" "")) nil nil nil 10 29 | "") 30 | (master "M'!7scE(!'@]D)!" 1 3 "NXT_Analog_Sensor_Polled" "" "" "" "" 31 | "" 5 nil 32 | (("Initialize" "" "" 3 16 nil nil "b'!hpcE(!'@]D)!" "") 33 | ("Get_Raw_Reading" "" "" 3 16 nil nil "['!}pcE(!'@]D)!" "")) 34 | nil nil nil 10 "") 35 | (master "7&!}vcE(!'@]D)!" 1 3 "NXT_Analog_Sensor" "" "" "" "" "" 5 36 | (("Converter" "ADC" "" "" "" "" 2 16 "W&!*ccE(!'@]D)!")) 37 | (("Initialize" "" "" 3 16 nil nil "_&!sscE(!'@]D)!" "") 38 | ("Get_Intensity" "" "" 3 16 nil nil "=&!+ucE(!'@]D)!" "") 39 | ("Get_Raw_Reading" "" "" 3 144 nil nil "E&!YtcE(!'@]D)!" 40 | "")) nil nil nil 10 "")) 41 | (customModel "6&!DwcE(!'@]D)!" 0 3 "Untitled" "" "" 17 "" "" 1.000000 1.000000 42 | (0 0 827 1168) (0 0 827 1168) 43 | (place "7&!}vcE(!'@]D)!" (3) "" 10 "8&!}vcE(!'@]D)!" 44 | (232 98 472 262) (227 93 477 267) (235 101 469 259) 1 0 45 | (nil 1 -24 2 18 12 18 0) "") 46 | (place "M'!7scE(!'@]D)!" (3) "" 10 "N'!7scE(!'@]D)!" 47 | (27 338 349 446) (22 333 354 451) (31 340 345 444) 1 0 48 | (nil 1 -24 0 18 12 18 0) "") 49 | (link "N'!7scE(!'@]D)!" "8&!}vcE(!'@]D)!" (229 337 287 261) 3 "" 50 | "" "%%" "%%" "" "" "" 1 0 (249 292 267 306) 51 | (225 329 225 329) (291 269 291 269) 0 0 "S'!MrcE(!'@]D)!" 52 | (229 261 287 337) (220 252 296 346) (248 291 268 307) 2 0 53 | (nil 1 -12 32 18 12 18 18) "") 54 | (place "j&!MicE(!'@]D)!" (3) "" 10 "'(! 34 | for spec ("hardware_Configuration") use "hardware_configuration_ble.ads"; 35 | for body ("Remote_Control") use "remote_control_adafruit_ble_app.adb"; 36 | when "PF_IR_8879" => 37 | for spec ("hardware_Configuration") use "hardware_configuration_ir.ads"; 38 | for body ("Remote_Control") use "remote_control_ir_pf8879.adb"; 39 | end case; 40 | end Naming; 41 | 42 | App_Switches := ""; 43 | 44 | package Compiler is 45 | for Local_Configuration_Pragmas use "gnat.adc"; 46 | case App_BUILD is 47 | when "Production" => 48 | App_Switches := ("-g", "-O3", "-gnatp", "-gnatn"); 49 | when "Debug" => 50 | App_Switches := ("-g", "-O0", "-gnata", "-fcallgraph-info=su"); 51 | end case; 52 | for Default_Switches ("ada") use Compiler'Default_Switches ("Ada") & 53 | App_Switches & 54 | ("-gnatwa", "-gnatQ", "-gnatw.X", "-gnaty", "-gnatyO", "-gnatyM120", 55 | "-ffunction-sections", "-fdata-sections"); 56 | end Compiler; 57 | 58 | 59 | package Ide is 60 | for Gnat use "arm-eabi-gnat"; 61 | for Gnatlist use "arm-eabi-gnatls"; 62 | for Debugger_Command use "arm-eabi-gdb"; 63 | for Program_Host use "localhost:4242"; 64 | for Communication_Protocol use "remote"; 65 | for Connection_Tool use "st-util"; 66 | end Ide; 67 | 68 | package CodePeer is 69 | for Switches use ("-gnateT=" & project'Project_Dir & "/arm-elf-target.atp"); 70 | end CodePeer; 71 | 72 | end RC_Car; 73 | -------------------------------------------------------------------------------- /src/ble/adafruit-ble_msg_utils.adb: -------------------------------------------------------------------------------- 1 | with System; 2 | with Ada.Unchecked_Conversion; 3 | with Ada.Numerics.Generic_Elementary_Functions; 4 | 5 | package body AdaFruit.BLE_Msg_Utils is 6 | 7 | Radians_To_Degrees : constant := 180.0 / Ada.Numerics.Pi; 8 | 9 | package Float_Elementary_Functions is new Ada.Numerics.Generic_Elementary_Functions (Float); 10 | use Float_Elementary_Functions; 11 | 12 | function Copysign (X, Y : Float) return Float with Inline; 13 | -- returns a value with the magnitude of X and the sign of Y 14 | 15 | --------- 16 | -- Yaw -- 17 | --------- 18 | 19 | function Yaw (Q : Quaternion_Data) return Float is 20 | Siny_Cosp : constant Float := +2.0 * (Q.W * Q.Z + Q.X * Q.Y); 21 | Cosy_Cosp : constant Float := +1.0 - 2.0 * (Q.Y * Q.Y + Q.Z * Q.Z); 22 | Result : Float; 23 | begin 24 | Result := Arctan (Siny_Cosp, Cosy_Cosp); 25 | Result := Result * Radians_To_Degrees; 26 | return Result; 27 | end Yaw; 28 | 29 | ---------- 30 | -- Roll -- 31 | ---------- 32 | 33 | function Roll (Q : Quaternion_Data) return Float is 34 | Sinr_Cosp : constant Float := 2.0 * (Q.W * Q.X + Q.Y * Q.Z); 35 | Cosr_Cosp : constant Float := 1.0 - 2.0 * (Q.X * Q.X + Q.Y * Q.Y); 36 | Result : Float; 37 | begin 38 | Result := Arctan (Sinr_Cosp, Cosr_Cosp); 39 | Result := Result * Radians_To_Degrees; 40 | return Result; 41 | end Roll; 42 | 43 | ----------- 44 | -- Pitch -- 45 | ----------- 46 | 47 | function Pitch (Q : Quaternion_Data) return Float is 48 | Sinp : constant Float := +2.0 * (Q.W * Q.Y - Q.Z * Q.X); 49 | Result : Float; 50 | begin 51 | if abs (Sinp) >= 1.0 then 52 | Result := Copysign (Ada.Numerics.Pi / 2.0, Sinp); 53 | else 54 | Result := Arcsin (Sinp); 55 | end if; 56 | Result := Result * Radians_To_Degrees; 57 | return Result; 58 | end Pitch; 59 | 60 | -------------- 61 | -- Copysign -- 62 | -------------- 63 | 64 | function Copysign (X, Y : Float) return Float is 65 | Result : Float := X; 66 | begin 67 | if Y >= 0.0 then -- positive result 68 | if Result < 0.0 then 69 | Result := -Result; 70 | end if; 71 | else -- negative result 72 | if Result >= 0.0 then 73 | Result := -Result; 74 | end if; 75 | end if; 76 | return Result; 77 | end Copysign; 78 | 79 | --------------------------------------- 80 | -- Parse_AdaFruit_Controller_Message -- 81 | --------------------------------------- 82 | 83 | procedure Parse_AdaFruit_Controller_Message 84 | (Input : String; 85 | Msg_Kind : Character; 86 | Result : out Payload; 87 | Success : out Boolean) 88 | is 89 | Payload_Length : constant Integer := Payload'Size / System.Storage_Unit; 90 | -- Message payloads are always composed of bytes so we know the number 91 | -- of bits is a integral multiple of a storage units' size, hence the 92 | -- simple division. 93 | 94 | Start : Integer range Input'First - 1 .. Input'Last := Input'First - 1; 95 | 96 | type Payload_Reference is access all Payload with Storage_Size => 0; 97 | 98 | function As_Payload_Reference is new Ada.Unchecked_Conversion 99 | (Source => System.Address, Target => Payload_Reference); 100 | 101 | begin 102 | Success := False; 103 | -- find header (eg "!G") in Input 104 | for K in Input'First .. Input'Last - 1 loop 105 | if Input (K) = '!' and Input (K + 1) = Msg_Kind then 106 | Start := K; 107 | exit; 108 | end if; 109 | end loop; 110 | if Start = Input'First - 1 then -- didn't find requested msg kind's header 111 | return; 112 | end if; 113 | if Start > Input'Last - Payload_Length + 1 then -- not enough data after the header 114 | return; 115 | end if; 116 | Success := True; 117 | Start := Start + 2; -- skip the 2-byte header 118 | Result := As_Payload_Reference (Input (Start)'Address).all; 119 | end Parse_AdaFruit_Controller_Message; 120 | 121 | end AdaFruit.BLE_Msg_Utils; 122 | -------------------------------------------------------------------------------- /src/ble/adafruit-ble_msg_utils.ads: -------------------------------------------------------------------------------- 1 | with Interfaces; 2 | 3 | package AdaFruit.BLE_Msg_Utils is 4 | 5 | type Three_Axes_Data is record 6 | X, Y, Z : Float; 7 | end record with 8 | Alignment => 1; 9 | 10 | type Quaternion_Data is record 11 | X, Y, Z, W : Float; 12 | end record with 13 | Alignment => 1; 14 | 15 | -- for the following three functions, see the Wikipedia entry: 16 | -- https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_Code_2 17 | function Pitch (Q : Quaternion_Data) return Float with Inline; 18 | function Roll (Q : Quaternion_Data) return Float with Inline; 19 | function Yaw (Q : Quaternion_Data) return Float with Inline; 20 | 21 | type Button_Data is record 22 | Button_Number : Interfaces.Unsigned_8; 23 | Value : Character; 24 | end record with 25 | Alignment => 1, 26 | Object_Size => 16; 27 | 28 | type Color_Data is record 29 | R, G, B : Interfaces.Unsigned_8; 30 | end record with 31 | Alignment => 1, 32 | Object_Size => 24; 33 | 34 | Gyro_Msg : constant Character := 'G'; 35 | Quaternion_Msg : constant Character := 'Q'; 36 | Accelerometer_Msg : constant Character := 'A'; 37 | Magnetometer_Msg : constant Character := 'M'; 38 | Button_Msg : constant Character := 'B'; 39 | Color_Picker_Msg : constant Character := 'C'; 40 | 41 | Quaternion_Msg_Length : constant := 18; 42 | Accelerometer_Msg_Length : constant := 14; 43 | Gyro_Msg_Length : constant := 14; 44 | Magnetometer_Msg_Length : constant := 14; 45 | Button_Msg_Length : constant := 4; 46 | Color_Picker_Msg_Length : constant := 5; 47 | 48 | generic 49 | type Payload is private; 50 | procedure Parse_AdaFruit_Controller_Message 51 | (Input : String; 52 | Msg_Kind : Character; 53 | Result : out Payload; 54 | Success : out Boolean); 55 | 56 | end AdaFruit.BLE_Msg_Utils; 57 | -------------------------------------------------------------------------------- /src/ble/adafruit-bluefruit_le_friend.adb: -------------------------------------------------------------------------------- 1 | with STM32.Device; use STM32.Device; 2 | 3 | package body AdaFruit.Bluefruit_LE_Friend is 4 | 5 | --------------- 6 | -- Configure -- 7 | --------------- 8 | 9 | procedure Configure 10 | (This : in out Bluefruit_LE_Transceiver; 11 | Mode_Pin : GPIO_Point) 12 | is 13 | Configuration : GPIO_Port_Configuration; 14 | begin 15 | This.Mode_Pin := Mode_Pin; 16 | 17 | Enable_Clock (Mode_Pin); 18 | Configuration := (Mode_Out, 19 | Output_Type => Push_Pull, 20 | Speed => Speed_100MHz, -- arbitrary 21 | Resistors => Pull_Down); 22 | Mode_Pin.Configure_IO (Configuration); 23 | end Configure; 24 | 25 | -------------- 26 | -- Set_Mode -- 27 | -------------- 28 | 29 | procedure Set_Mode 30 | (This : in out Bluefruit_LE_Transceiver; 31 | Mode : Modes) 32 | is 33 | begin 34 | if Mode = Command then 35 | This.Mode_Pin.Set; 36 | else 37 | This.Mode_Pin.Clear; 38 | end if; 39 | end Set_Mode; 40 | 41 | ------------------ 42 | -- Current_Mode -- 43 | ------------------ 44 | 45 | function Current_Mode (This : Bluefruit_LE_Transceiver) return Modes is 46 | begin 47 | return (if This.Mode_Pin.Set then Command else Data); 48 | end Current_Mode; 49 | 50 | --------- 51 | -- Put -- 52 | --------- 53 | 54 | procedure Put 55 | (This : in out Bluefruit_LE_Transceiver; 56 | Data : Character) 57 | is 58 | begin 59 | Write (This.Port.all, Data); 60 | end Put; 61 | 62 | --------- 63 | -- Put -- 64 | --------- 65 | 66 | procedure Put 67 | (This : in out Bluefruit_LE_Transceiver; 68 | Data : String) 69 | is 70 | begin 71 | for Next_Char of Data loop 72 | Write (This.Port.all, Next_Char); 73 | end loop; 74 | end Put; 75 | 76 | --------- 77 | -- Get -- 78 | --------- 79 | 80 | procedure Get 81 | (This : in out Bluefruit_LE_Transceiver; 82 | Data : out Character) 83 | is 84 | begin 85 | Read (This.Port.all, Data); 86 | end Get; 87 | 88 | --------- 89 | -- Get -- 90 | --------- 91 | 92 | procedure Get 93 | (This : in out Bluefruit_LE_Transceiver; 94 | Data : out String; 95 | Last : out Natural; 96 | EOM : Character) 97 | is 98 | Next_Received : Character; 99 | begin 100 | Last := Data'First - 1; 101 | for Index in Data'Range loop 102 | Read (This.Port.all, Next_Received); 103 | exit when Next_Received = EOM; 104 | Data (Index) := Next_Received; 105 | Last := Index; 106 | end loop; 107 | end Get; 108 | 109 | --------- 110 | -- Get -- 111 | --------- 112 | 113 | procedure Get 114 | (This : in out Bluefruit_LE_Transceiver; 115 | Data : out String) 116 | is 117 | begin 118 | for Index in Data'Range loop 119 | Read (This.Port.all, Data (Index)); 120 | end loop; 121 | end Get; 122 | 123 | end AdaFruit.Bluefruit_LE_Friend; 124 | -------------------------------------------------------------------------------- /src/ble/adafruit-bluefruit_le_friend.ads: -------------------------------------------------------------------------------- 1 | -- See https://www.adafruit.com/product/2479 for the product and links to the 2 | -- (free) smarphone app provided by AdaFruit. 3 | 4 | -- The red Mode LED on the breakout board will be blinking 3 times with a 5 | -- 3-second pause when in "CMD" mode, or 2-times when in "UART Data" mode. 6 | 7 | -- The blue LED indicates a Bluetooth connection when on. 8 | 9 | -- The UART Data mode uses hardware flow control so you must clear the CTS pin 10 | -- to enable the TXO pin. See the discussion on this page: 11 | -- 12 | -- https://learn.adafruit.com/introducing-the-adafruit-bluefruit-le-uart-friend/pinouts 13 | 14 | with STM32.GPIO; use STM32.GPIO; 15 | 16 | generic 17 | 18 | type Transport_Media (<>) is limited private; 19 | -- This is the means of communicating between the BLE device and the MCU. 20 | -- The AdaFruit BLE Friend has two variants based on this medium: one 21 | -- that uses SPI and one that uses UART to communicate with the MCU. These 22 | -- generic formal parameters factor out that selection so that this one 23 | -- driver can be used with either medium (once instantiated). 24 | 25 | with procedure Read 26 | (This : in out Transport_Media; 27 | Value : out Character) is <>; 28 | 29 | with procedure Write 30 | (This : in out Transport_Media; 31 | Value : Character) is <>; 32 | 33 | package AdaFruit.Bluefruit_LE_Friend is 34 | 35 | type Bluefruit_LE_Transceiver (Port : not null access Transport_Media) is 36 | tagged limited private; 37 | 38 | procedure Configure 39 | (This : in out Bluefruit_LE_Transceiver; 40 | Mode_Pin : GPIO_Point); 41 | 42 | type Modes is (Command, Data); 43 | 44 | procedure Set_Mode 45 | (This : in out Bluefruit_LE_Transceiver; 46 | Mode : Modes) 47 | with Post => Current_Mode (This) = Mode; 48 | -- Note this will override the corresponding on-board switch setting 49 | 50 | function Current_Mode (This : Bluefruit_LE_Transceiver) return Modes; 51 | 52 | procedure Put 53 | (This : in out Bluefruit_LE_Transceiver; 54 | Data : Character); 55 | 56 | procedure Put 57 | (This : in out Bluefruit_LE_Transceiver; 58 | Data : String); 59 | 60 | procedure Get 61 | (This : in out Bluefruit_LE_Transceiver; 62 | Data : out Character); 63 | 64 | procedure Get 65 | (This : in out Bluefruit_LE_Transceiver; 66 | Data : out String; 67 | Last : out Natural; 68 | EOM : Character); 69 | -- Fills the buffer. The value of Last will be the last index used when 70 | -- filling. Filling stops when either the buffer is full or the EOM char 71 | -- is detected. The EOM char is not included in the buffer. 72 | 73 | procedure Get 74 | (This : in out Bluefruit_LE_Transceiver; 75 | Data : out String); 76 | -- Fills the entire buffer before returning. 77 | 78 | private 79 | 80 | type Bluefruit_LE_Transceiver (Port : not null access Transport_Media) is 81 | tagged limited record 82 | Mode_Pin : GPIO_Point; 83 | end record; 84 | 85 | end AdaFruit.Bluefruit_LE_Friend; 86 | -------------------------------------------------------------------------------- /src/ble/bluefruit_le_friend_usart.ads: -------------------------------------------------------------------------------- 1 | with Serial_IO.Interrupt_Driven; use Serial_IO.Interrupt_Driven; 2 | 3 | with AdaFruit.Bluefruit_LE_Friend; 4 | 5 | package BlueFruit_LE_Friend_USART is new AdaFruit.Bluefruit_LE_Friend 6 | (Transport_Media => Serial_Port, 7 | Read => Get, 8 | Write => Put); 9 | -------------------------------------------------------------------------------- /src/ble/serial_io-interrupt_driven.adb: -------------------------------------------------------------------------------- 1 | ------------------------------------------------------------------------------ 2 | -- -- 3 | -- Copyright (C) 2015-2018, AdaCore -- 4 | -- -- 5 | -- Redistribution and use in source and binary forms, with or without -- 6 | -- modification, are permitted provided that the following conditions are -- 7 | -- met: -- 8 | -- 1. Redistributions of source code must retain the above copyright -- 9 | -- notice, this list of conditions and the following disclaimer. -- 10 | -- 2. Redistributions in binary form must reproduce the above copyright -- 11 | -- notice, this list of conditions and the following disclaimer in -- 12 | -- the documentation and/or other materials provided with the -- 13 | -- distribution. -- 14 | -- 3. Neither the name of the copyright holder nor the names of its -- 15 | -- contributors may be used to endorse or promote products derived -- 16 | -- from this software without specific prior written permission. -- 17 | -- -- 18 | -- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -- 19 | -- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -- 20 | -- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -- 21 | -- A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -- 22 | -- HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -- 23 | -- SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -- 24 | -- LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -- 25 | -- DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -- 26 | -- THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -- 27 | -- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -- 28 | -- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -- 29 | -- -- 30 | ------------------------------------------------------------------------------ 31 | 32 | with STM32.Device; use STM32.Device; 33 | 34 | package body Serial_IO.Interrupt_Driven is 35 | 36 | --------- 37 | -- Put -- 38 | --------- 39 | 40 | overriding 41 | procedure Put (This : in out Serial_Port; Data : Character) is 42 | begin 43 | This.Controller.Put (Data); 44 | end Put; 45 | 46 | --------- 47 | -- Get -- 48 | --------- 49 | 50 | overriding 51 | procedure Get (This : in out Serial_Port; Data : out Character) is 52 | begin 53 | Enable_Interrupts (This.Transceiver.all, Received_Data_Not_Empty); 54 | This.Controller.Get (Data); 55 | end Get; 56 | 57 | ---------------- 58 | -- IO_Manager -- 59 | ---------------- 60 | 61 | protected body IO_Manager is 62 | 63 | ------------------------- 64 | -- Handle_Transmission -- 65 | ------------------------- 66 | 67 | procedure Handle_Transmission is 68 | begin 69 | Serial_IO.Put (Serial_IO.Device (Port.all), Outgoing); 70 | Disable_Interrupts (Port.Transceiver.all, Source => Transmission_Complete); 71 | end Handle_Transmission; 72 | 73 | ---------------------- 74 | -- Handle_Reception -- 75 | ---------------------- 76 | 77 | procedure Handle_Reception is 78 | begin 79 | Serial_IO.Get (Serial_IO.Device (Port.all), Incoming); 80 | loop 81 | exit when not Status (Port.Transceiver.all, Read_Data_Register_Not_Empty); 82 | end loop; 83 | Disable_Interrupts (Port.Transceiver.all, Source => Received_Data_Not_Empty); 84 | end Handle_Reception; 85 | 86 | ----------------- 87 | -- IRQ_Handler -- 88 | ----------------- 89 | 90 | procedure IRQ_Handler is 91 | begin 92 | -- check for data arrival 93 | if Status (Port.Transceiver.all, Read_Data_Register_Not_Empty) and 94 | Interrupt_Enabled (Port.Transceiver.all, Received_Data_Not_Empty) 95 | then 96 | Handle_Reception; 97 | Clear_Status (Port.Transceiver.all, Read_Data_Register_Not_Empty); 98 | Incoming_Data_Available := True; 99 | end if; 100 | 101 | -- check for transmission ready 102 | if Status (Port.Transceiver.all, Transmission_Complete_Indicated) and 103 | Interrupt_Enabled (Port.Transceiver.all, Transmission_Complete) 104 | then 105 | Handle_Transmission; 106 | Clear_Status (Port.Transceiver.all, Transmission_Complete_Indicated); 107 | Transmission_Pending := False; 108 | end if; 109 | end IRQ_Handler; 110 | 111 | --------- 112 | -- Put -- 113 | --------- 114 | 115 | entry Put (Datum : Character) when not Transmission_Pending is 116 | begin 117 | Transmission_Pending := True; 118 | Outgoing := Datum; 119 | Enable_Interrupts (Port.Transceiver.all, Transmission_Complete); 120 | end Put; 121 | 122 | --------- 123 | -- Get -- 124 | --------- 125 | 126 | entry Get (Datum : out Character) when Incoming_Data_Available is 127 | begin 128 | Datum := Incoming; 129 | Incoming_Data_Available := False; 130 | end Get; 131 | 132 | end IO_Manager; 133 | 134 | end Serial_IO.Interrupt_Driven; 135 | -------------------------------------------------------------------------------- /src/ble/serial_io-interrupt_driven.ads: -------------------------------------------------------------------------------- 1 | with Ada.Interrupts; use Ada.Interrupts; 2 | 3 | package Serial_IO.Interrupt_Driven is 4 | pragma Elaborate_Body; 5 | 6 | type Serial_Port (IRQ : Interrupt_ID) is new Serial_IO.Device with private; 7 | -- A serial port that uses interrupts for I/O. Extends the serial port 8 | -- abstraction that is itself a wrapper for the USARTs hardware. 9 | 10 | overriding 11 | procedure Put (This : in out Serial_Port; Data : Character) with Inline; 12 | -- Non-blocking, ie the caller returns while the character is going out. 13 | -- Will not interfere with any other I/O on the same device. 14 | 15 | overriding 16 | procedure Get (This : in out Serial_Port; Data : out Character) with Inline; 17 | -- Blocks the caller until a character is available! 18 | -- Will not interfere with any other I/O on the same device. 19 | 20 | private 21 | 22 | -- The protected type defining the interrupt-based I/O for sending and 23 | -- receiving via the USART attached to the serial port designated by 24 | -- Port. Each serial port object of the type defined by this package has 25 | -- a component of this protected type. 26 | protected type IO_Manager 27 | (IRQ : Interrupt_ID; 28 | Port : access Serial_Port) 29 | is 30 | pragma Interrupt_Priority; 31 | 32 | entry Put (Datum : Character); 33 | 34 | entry Get (Datum : out Character); 35 | 36 | private 37 | 38 | Outgoing : Character; 39 | Incoming : Character; 40 | 41 | Incoming_Data_Available : Boolean := False; 42 | Transmission_Pending : Boolean := False; 43 | 44 | procedure Handle_Transmission with Inline; 45 | procedure Handle_Reception with Inline; 46 | 47 | procedure IRQ_Handler with Attach_Handler => IRQ; 48 | -- The one interrupt handler for both sending and receiving. Calls the 49 | -- respective Handle_* routines above for each incoming or outgoing 50 | -- character interrupt on the USART. 51 | 52 | end IO_Manager; 53 | 54 | type Serial_Port (IRQ : Interrupt_ID) is new Serial_IO.Device with record 55 | Controller : IO_Manager (IRQ, Serial_Port'Access); 56 | -- Note that the access discriminant on the protected type provides the 57 | -- Controller with a view to the Serial_IO.Device components inherited 58 | -- by this type extension (as well as components introduced by the 59 | -- extension itself, if any). 60 | end record; 61 | 62 | end Serial_IO.Interrupt_Driven; 63 | -------------------------------------------------------------------------------- /src/ble/serial_io.adb: -------------------------------------------------------------------------------- 1 | ------------------------------------------------------------------------------ 2 | -- -- 3 | -- Copyright (C) 2015-2016, AdaCore -- 4 | -- -- 5 | -- Redistribution and use in source and binary forms, with or without -- 6 | -- modification, are permitted provided that the following conditions are -- 7 | -- met: -- 8 | -- 1. Redistributions of source code must retain the above copyright -- 9 | -- notice, this list of conditions and the following disclaimer. -- 10 | -- 2. Redistributions in binary form must reproduce the above copyright -- 11 | -- notice, this list of conditions and the following disclaimer in -- 12 | -- the documentation and/or other materials provided with the -- 13 | -- distribution. -- 14 | -- 3. Neither the name of the copyright holder nor the names of its -- 15 | -- contributors may be used to endorse or promote products derived -- 16 | -- from this software without specific prior written permission. -- 17 | -- -- 18 | -- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -- 19 | -- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -- 20 | -- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -- 21 | -- A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -- 22 | -- HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -- 23 | -- SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -- 24 | -- LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -- 25 | -- DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -- 26 | -- THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -- 27 | -- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -- 28 | -- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -- 29 | -- -- 30 | ------------------------------------------------------------------------------ 31 | 32 | with STM32.Device; use STM32.Device; 33 | 34 | with HAL; 35 | 36 | package body Serial_IO is 37 | 38 | ---------------- 39 | -- Initialize -- 40 | ---------------- 41 | 42 | procedure Initialize 43 | (This : in out Device; 44 | Transceiver : not null access USART; 45 | Transceiver_AF : GPIO_Alternate_Function; 46 | Tx_Pin : GPIO_Point; 47 | Rx_Pin : GPIO_Point; 48 | CTS_Pin : GPIO_Point; 49 | RTS_Pin : GPIO_Point) 50 | is 51 | Configuration : GPIO_Port_Configuration; 52 | IO_Pins : constant GPIO_Points := Rx_Pin & Tx_Pin; 53 | begin 54 | This.Transceiver := Transceiver; 55 | This.Tx_Pin := Tx_Pin; 56 | This.Rx_Pin := Rx_Pin; 57 | This.CTS_Pin := CTS_Pin; 58 | This.RTS_Pin := RTS_Pin; 59 | 60 | Enable_Clock (Transceiver.all); 61 | 62 | Enable_Clock (IO_Pins); 63 | 64 | Configuration := (Mode_AF, 65 | AF => Transceiver_AF, 66 | AF_Speed => Speed_50MHz, 67 | AF_Output_Type => Push_Pull, 68 | Resistors => Pull_Up); 69 | 70 | Configure_IO (IO_Pins, Configuration); 71 | 72 | Enable_Clock (RTS_Pin & CTS_Pin); 73 | 74 | Configuration := (Mode_In, Resistors => Pull_Up); 75 | Configure_IO (RTS_Pin, Configuration); 76 | 77 | Configuration := (Mode_Out, 78 | Speed => Speed_50MHz, 79 | Output_Type => Push_Pull, 80 | Resistors => Pull_Up); 81 | 82 | Configure_IO (CTS_Pin, Configuration); 83 | end Initialize; 84 | 85 | --------------- 86 | -- Configure -- 87 | --------------- 88 | 89 | procedure Configure 90 | (This : in out Device; 91 | Baud_Rate : Baud_Rates; 92 | Parity : Parities := No_Parity; 93 | Data_Bits : Word_Lengths := Word_Length_8; 94 | End_Bits : Stop_Bits := Stopbits_1; 95 | Control : Flow_Control := No_Flow_Control) 96 | is 97 | begin 98 | Disable (This.Transceiver.all); 99 | 100 | Set_Baud_Rate (This.Transceiver.all, Baud_Rate); 101 | Set_Mode (This.Transceiver.all, Tx_Rx_Mode); 102 | Set_Stop_Bits (This.Transceiver.all, End_Bits); 103 | Set_Word_Length (This.Transceiver.all, Data_Bits); 104 | Set_Parity (This.Transceiver.all, Parity); 105 | Set_Flow_Control (This.Transceiver.all, Control); 106 | 107 | Enable (This.Transceiver.all); 108 | end Configure; 109 | 110 | ------------- 111 | -- Set_CTS -- 112 | ------------- 113 | 114 | procedure Set_CTS (This : in out Device; Value : Boolean) is 115 | begin 116 | This.CTS_Pin.Drive (Value); 117 | end Set_CTS; 118 | 119 | ------------- 120 | -- Set_RTS -- 121 | ------------- 122 | 123 | procedure Set_RTS (This : in out Device; Value : Boolean) is 124 | begin 125 | This.RTS_Pin.Drive (Value); 126 | end Set_RTS; 127 | 128 | --------- 129 | -- Put -- 130 | --------- 131 | 132 | procedure Put (This : in out Device; Data : Character) is 133 | begin 134 | Transmit (This.Transceiver.all, Character'Pos (Data)); 135 | end Put; 136 | 137 | --------- 138 | -- Get -- 139 | --------- 140 | 141 | procedure Get (This : in out Device; Data : out Character) is 142 | Received : HAL.UInt9; 143 | begin 144 | Receive (This.Transceiver.all, Received); 145 | Data := Character'Val (Received); 146 | end Get; 147 | 148 | end Serial_IO; 149 | -------------------------------------------------------------------------------- /src/ble/serial_io.ads: -------------------------------------------------------------------------------- 1 | ------------------------------------------------------------------------------ 2 | -- -- 3 | -- Copyright (C) 2015-2016, AdaCore -- 4 | -- -- 5 | -- Redistribution and use in source and binary forms, with or without -- 6 | -- modification, are permitted provided that the following conditions are -- 7 | -- met: -- 8 | -- 1. Redistributions of source code must retain the above copyright -- 9 | -- notice, this list of conditions and the following disclaimer. -- 10 | -- 2. Redistributions in binary form must reproduce the above copyright -- 11 | -- notice, this list of conditions and the following disclaimer in -- 12 | -- the documentation and/or other materials provided with the -- 13 | -- distribution. -- 14 | -- 3. Neither the name of the copyright holder nor the names of its -- 15 | -- contributors may be used to endorse or promote products derived -- 16 | -- from this software without specific prior written permission. -- 17 | -- -- 18 | -- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -- 19 | -- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -- 20 | -- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -- 21 | -- A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -- 22 | -- HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -- 23 | -- SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -- 24 | -- LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -- 25 | -- DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -- 26 | -- THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -- 27 | -- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -- 28 | -- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -- 29 | -- -- 30 | ------------------------------------------------------------------------------ 31 | 32 | with STM32; use STM32; 33 | with STM32.GPIO; use STM32.GPIO; 34 | with STM32.USARTs; use STM32.USARTs; 35 | 36 | package Serial_IO is 37 | 38 | type Device is tagged limited private; 39 | 40 | procedure Initialize 41 | (This : in out Device; 42 | Transceiver : not null access USART; 43 | Transceiver_AF : GPIO_Alternate_Function; 44 | Tx_Pin : GPIO_Point; 45 | Rx_Pin : GPIO_Point; 46 | CTS_Pin : GPIO_Point; 47 | RTS_Pin : GPIO_Point); 48 | 49 | procedure Configure 50 | (This : in out Device; 51 | Baud_Rate : Baud_Rates; 52 | Parity : Parities := No_Parity; 53 | Data_Bits : Word_Lengths := Word_Length_8; 54 | End_Bits : Stop_Bits := Stopbits_1; 55 | Control : Flow_Control := No_Flow_Control); 56 | 57 | procedure Set_CTS (This : in out Device; Value : Boolean) with Inline; 58 | procedure Set_RTS (This : in out Device; Value : Boolean) with Inline; 59 | 60 | procedure Put (This : in out Device; Data : Character) with Inline; 61 | 62 | procedure Get (This : in out Device; Data : out Character) with Inline; 63 | 64 | private 65 | 66 | type Device is tagged limited record 67 | Transceiver : access USART; 68 | Tx_Pin : GPIO_Point; 69 | Rx_Pin : GPIO_Point; 70 | CTS_Pin : GPIO_Point; 71 | RTS_Pin : GPIO_Point; 72 | end record; 73 | 74 | end Serial_IO; 75 | -------------------------------------------------------------------------------- /src/collision_detection.adb: -------------------------------------------------------------------------------- 1 | with STM32.Board; use STM32.Board; 2 | 3 | package body Collision_Detection 4 | with SPARK_Mode 5 | is 6 | 7 | function Moving_Forward 8 | (Current_Direction : Remote_Control.Travel_Directions; 9 | Current_Speed : Float) 10 | return Boolean 11 | with Inline; 12 | 13 | function Exclusion_Zone_Violated (Sonar_Reading : Centimeters) return Boolean 14 | with Inline; 15 | 16 | ----------- 17 | -- Check -- 18 | ----------- 19 | 20 | procedure Check 21 | (Current_Direction : Remote_Control.Travel_Directions; 22 | Current_Speed : Float; 23 | Collision_Imminent : out Boolean) 24 | is 25 | Reading : Centimeters; 26 | IO_Successful : Boolean; 27 | begin 28 | Orange_LED.Clear; 29 | Collision_Imminent := False; -- default 30 | if Moving_Forward (Current_Direction, Current_Speed) then 31 | Vehicle.Sonar.Get_Distance (Reading, IO_Successful); 32 | if not IO_Successful then 33 | Orange_LED.Set; 34 | return; 35 | else 36 | Collision_Imminent := Exclusion_Zone_Violated (Reading); 37 | end if; 38 | end if; 39 | end Check; 40 | 41 | -------------------- 42 | -- Moving_Forward -- 43 | -------------------- 44 | 45 | function Moving_Forward 46 | (Current_Direction : Remote_Control.Travel_Directions; 47 | Current_Speed : Float) 48 | return Boolean 49 | is 50 | use Remote_Control; 51 | begin 52 | return Current_Direction = Forward and Current_Speed > 0.0; 53 | end Moving_Forward; 54 | 55 | ----------------------------- 56 | -- Exclusion_Zone_Violated -- 57 | ----------------------------- 58 | 59 | function Exclusion_Zone_Violated (Sonar_Reading : Centimeters) return Boolean is 60 | Min_Distance : constant Centimeters := 27 + Vehicle.Sonar_Offset_From_Front; 61 | -- The minimum value read from the sonar sensor indicating an obstacle 62 | -- ahead that will require us to stop to avoid a collision. Empirically 63 | -- determined, and approximate. Meant to be sufficient when the vehicle 64 | -- is traveling at high speed (ie, to give time to stop). Note that too 65 | -- large a number will cause the vehicle to stop more often than expected 66 | -- by drivers. 67 | begin 68 | if Sonar_Reading = NXT.Ultrasonic_Sensors.Nothing_Detected then 69 | return False; 70 | else 71 | return Sonar_Reading <= Min_Distance; 72 | end if; 73 | end Exclusion_Zone_Violated; 74 | 75 | end Collision_Detection; 76 | -------------------------------------------------------------------------------- /src/collision_detection.ads: -------------------------------------------------------------------------------- 1 | with Remote_Control; 2 | with Vehicle; 3 | with NXT.Ultrasonic_Sensors; use NXT.Ultrasonic_Sensors; 4 | 5 | package Collision_Detection 6 | with SPARK_Mode 7 | is 8 | pragma Unevaluated_Use_Of_Old (Allow); 9 | 10 | procedure Check 11 | (Current_Direction : Remote_Control.Travel_Directions; 12 | Current_Speed : Float; 13 | Collision_Imminent : out Boolean) 14 | with 15 | Pre => Vehicle.Sonar.Configured and then 16 | Vehicle.Sonar.Enabled and then 17 | Vehicle.Sonar.Current_Scan_Mode = Continuous, 18 | Post => Vehicle.Sonar.Configured and then 19 | Vehicle.Sonar.Enabled and then 20 | Vehicle.Sonar.Current_Scan_Mode = Vehicle.Sonar.Current_Scan_Mode'Old; 21 | 22 | end Collision_Detection; 23 | -------------------------------------------------------------------------------- /src/engine_control.adb: -------------------------------------------------------------------------------- 1 | with Global_Initialization; 2 | with Ada.Real_Time; use Ada.Real_Time; 3 | with NXT.Motors; use NXT.Motors; 4 | with Remote_Control; use Remote_Control; 5 | with Collision_Detection; 6 | with Vehicle; use Vehicle; 7 | with STM32.Board; use STM32.Board; 8 | with NXT.Ultrasonic_Sensors; use NXT.Ultrasonic_Sensors; 9 | 10 | package body Engine_Control with 11 | SPARK_Mode -- => Off -- for now... 12 | is 13 | 14 | Period : constant Time_Span := Milliseconds (System_Configuration.Engine_Control_Period); 15 | 16 | Vector : Remote_Control.Travel_Vector with Atomic, Async_Readers, Async_Writers; 17 | -- we must declare this here, and access it as shown in the task body, for SPARK 18 | 19 | procedure Indicate_Emergency_Stopped; 20 | -- The Red LED is turned on and the Green LED is turned off. 21 | 22 | procedure Indicate_Running; 23 | -- The Red LED is turned off and the Green LED is toggled. 24 | 25 | procedure Apply 26 | (Direction : Remote_Control.Travel_Directions; 27 | Power : Remote_Control.Percentage); 28 | 29 | procedure Emergency_Stop; 30 | 31 | type Controller_States is (Running, Braked, Awaiting_Reversal); 32 | 33 | ---------------- 34 | -- Controller -- 35 | ---------------- 36 | 37 | task body Controller is 38 | Current_State : Controller_States := Running; 39 | Next_Release : Time; 40 | Requested_Direction : Remote_Control.Travel_Directions; 41 | Requested_Braking : Boolean; 42 | Requested_Power : Remote_Control.Percentage; 43 | Collision_Imminent : Boolean; 44 | Current_Speed : Float; 45 | begin 46 | Global_Initialization.Critical_Instant.Wait (Epoch => Next_Release); 47 | 48 | -- In the following loop, the call to get the requested vector does not 49 | -- block awaiting some change of input. The vectors are received as a 50 | -- continuous stream of values, often not changing from their previous 51 | -- values, rather than as a set of discrete commanded changes sent only 52 | -- when a new vector is commanded by the user. 53 | loop 54 | pragma Loop_Invariant (Configured (Vehicle.Sonar) and then 55 | Enabled (Vehicle.Sonar) and then 56 | Current_Scan_Mode (Vehicle.Sonar) = Continuous); 57 | 58 | Vector := Remote_Control.Requested_Vector; 59 | Requested_Direction := Vector.Direction; 60 | Requested_Braking := Vector.Emergency_Braking; 61 | Requested_Power := Vector.Power; 62 | Current_Speed := Vehicle.Speed; 63 | 64 | case Current_State is 65 | when Running => 66 | Collision_Detection.Check (Requested_Direction, Current_Speed, Collision_Imminent); 67 | if Collision_Imminent then 68 | Emergency_Stop; 69 | Indicate_Emergency_Stopped; 70 | Current_State := Awaiting_Reversal; 71 | elsif Requested_Braking then 72 | Emergency_Stop; 73 | Indicate_Emergency_Stopped; 74 | Current_State := Braked; 75 | else 76 | Apply (Requested_Direction, Requested_Power); 77 | Indicate_Running; 78 | end if; 79 | when Braked => 80 | if not Requested_Braking then 81 | Current_State := Running; 82 | end if; 83 | when Awaiting_Reversal => 84 | if Requested_Direction = Backward then 85 | Current_State := Running; 86 | end if; 87 | end case; 88 | 89 | Next_Release := Next_Release + Period; 90 | delay until Next_Release; 91 | end loop; 92 | end Controller; 93 | 94 | -------------------- 95 | -- Emergency_Stop -- 96 | -------------------- 97 | 98 | procedure Emergency_Stop is 99 | begin 100 | Engine.Stop; 101 | end Emergency_Stop; 102 | 103 | ----------- 104 | -- Apply -- 105 | ----------- 106 | 107 | procedure Apply 108 | (Direction : Remote_Control.Travel_Directions; 109 | Power : Remote_Control.Percentage) 110 | is 111 | begin 112 | if Direction /= Neither then 113 | Engine.Engage (Vehicle.To_Propulsion_Motor_Direction (Direction), Power); 114 | else 115 | Engine.Coast; 116 | end if; 117 | end Apply; 118 | 119 | -------------------------------- 120 | -- Indicate_Emergency_Stopped -- 121 | -------------------------------- 122 | 123 | procedure Indicate_Emergency_Stopped is 124 | begin 125 | Red_LED.Set; 126 | Green_LED.Clear; 127 | end Indicate_Emergency_Stopped; 128 | 129 | ---------------------- 130 | -- Indicate_Running -- 131 | ---------------------- 132 | 133 | procedure Indicate_Running is 134 | begin 135 | Red_LED.Clear; 136 | Green_LED.Toggle; 137 | end Indicate_Running; 138 | 139 | end Engine_Control; 140 | -------------------------------------------------------------------------------- /src/engine_control.ads: -------------------------------------------------------------------------------- 1 | with System_Configuration; 2 | 3 | package Engine_Control with 4 | SPARK_Mode 5 | is 6 | 7 | pragma Elaborate_Body; 8 | 9 | private 10 | 11 | task Controller with 12 | Storage_Size => 1 * 1024, 13 | Priority => System_Configuration.Engine_Control_Priority; 14 | 15 | end Engine_Control; 16 | -------------------------------------------------------------------------------- /src/global_initialization.adb: -------------------------------------------------------------------------------- 1 | package body Global_Initialization 2 | with SPARK_Mode 3 | is 4 | 5 | protected body Critical_Instant is 6 | 7 | ------------ 8 | -- Signal -- 9 | ------------ 10 | 11 | procedure Signal (Epoch : Time) is 12 | begin 13 | Signalled := True; 14 | Global_Epoch := Epoch; 15 | end Signal; 16 | 17 | ---------- 18 | -- Wait -- 19 | ---------- 20 | 21 | entry Wait (Epoch : out Time) when Signalled is 22 | begin 23 | Epoch := Global_Epoch; 24 | end Wait; 25 | 26 | end Critical_Instant; 27 | 28 | 29 | end Global_Initialization; 30 | -------------------------------------------------------------------------------- /src/global_initialization.ads: -------------------------------------------------------------------------------- 1 | -- This package provides a synchronization mechanism used to signal to the 2 | -- various tasks that the critical instant has arrived and that, as a result, 3 | -- their periodic execution can now commence (with the Time value specified for 4 | -- the epoch). 5 | 6 | with System_Configuration; 7 | with Ada.Real_Time; use Ada.Real_Time; 8 | 9 | package Global_Initialization 10 | with SPARK_Mode 11 | is 12 | 13 | protected Critical_Instant 14 | with Priority => System_Configuration.Highest_Priority 15 | is 16 | procedure Signal (Epoch : Time); 17 | -- signal completion of the global initialization sequence and specify 18 | -- the beginning of the epoch for task periodic processing 19 | 20 | entry Wait (Epoch : out Time); 21 | -- await completion of the global initialization sequence and get 22 | -- the beginning of the epoch for task periodic processing 23 | 24 | private 25 | Signalled : Boolean := False; 26 | Global_Epoch : Time := Time_First; -- overwritten by Signal 27 | end Critical_Instant; 28 | 29 | end Global_Initialization; 30 | -------------------------------------------------------------------------------- /src/hardware_configuration_ble.ads: -------------------------------------------------------------------------------- 1 | ------------------------------------------------------------------------------ 2 | -- -- 3 | -- Copyright (C) 2018, AdaCore -- 4 | -- -- 5 | -- Redistribution and use in source and binary forms, with or without -- 6 | -- modification, are permitted provided that the following conditions are -- 7 | -- met: -- 8 | -- 1. Redistributions of source code must retain the above copyright -- 9 | -- notice, this list of conditions and the following disclaimer. -- 10 | -- 2. Redistributions in binary form must reproduce the above copyright -- 11 | -- notice, this list of conditions and the following disclaimer in -- 12 | -- the documentation and/or other materials provided with the -- 13 | -- distribution. -- 14 | -- 3. Neither the name of STMicroelectronics nor the names of its -- 15 | -- contributors may be used to endorse or promote products derived -- 16 | -- from this software without specific prior written permission. -- 17 | -- -- 18 | -- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -- 19 | -- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -- 20 | -- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -- 21 | -- A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -- 22 | -- HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -- 23 | -- SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -- 24 | -- LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -- 25 | -- DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -- 26 | -- THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -- 27 | -- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -- 28 | -- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -- 29 | -- -- 30 | ------------------------------------------------------------------------------ 31 | 32 | -- This package declares all the hardware devices and related values on the 33 | -- STM32 board actually used by the application. The purpose is to specify 34 | -- them all in one place, so that changes and/or additions can be checked 35 | -- easily for conflicts. 36 | 37 | -- There are multiple versions of this package, depending on the remote 38 | -- receiver communication hardware. For example, there is one package for 39 | -- IR receivers and one for BLE. This is the package for the IR receiver. 40 | 41 | -- Only the remote receiver connections vary among the versions of this 42 | -- package, suggesting that the other values should be in a single, separate 43 | -- package to avoid duplication and thus potential inconsistencies over time. 44 | -- However, the ability to check for device and GPIO conflicts is so valuable 45 | -- as to override the desire to avoid uplication. 46 | 47 | with STM32; use STM32; 48 | with STM32.GPIO; use STM32.GPIO; 49 | with STM32.Timers; use STM32.Timers; 50 | with STM32.Device; use STM32.Device; 51 | 52 | with Ada.Interrupts.Names; use Ada.Interrupts.Names; 53 | with STM32.USARTs; use STM32.USARTs; 54 | 55 | package Hardware_Configuration is 56 | 57 | -- The hardware on the STM32 board used by the Ultrasonic Sonar sensor (on 58 | -- the NXT_Shield) 59 | 60 | Sonar_Clock_Frequency : constant := 9600; 61 | Sonar_Clock_Pin : GPIO_Point renames PB13; -- SCL 62 | Sonar_Data_Pin : GPIO_Point renames PB11; -- SDA 63 | -- The choice of pins is largely arbitrary because we are bit-banging the 64 | -- I/O instead of using an ob-board I2C device. Nonetheless, the internal 65 | -- pull-up resistor values are not the same across all pins. Specifically, 66 | -- PB10 and PB12 have approximately 11K pull-up resistors, whereas the 67 | -- other pins have approximately 40K pull-up resistors. See table 47 68 | -- "I/O Static Characteristics" in the STM32F405xx STM32F407xx Datasheet. 69 | 70 | -- The hardware on the STM32 board used by the two motors (on the 71 | -- NXT_Shield) 72 | 73 | Motor_PWM_Frequency : constant := 490; 74 | 75 | Motor1_Encoder_Input1 : GPIO_Point renames PA15; 76 | Motor1_Encoder_Input2 : GPIO_Point renames PB3; 77 | Motor1_Encoder_Timer : constant access Timer := Timer_2'Access; 78 | Motor1_Encoder_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_TIM2_1; 79 | Motor1_PWM_Timer : constant access Timer := Timer_4'Access; 80 | Motor1_PWM_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_TIM4_2; 81 | Motor1_PWM_Output : GPIO_Point renames PB6; 82 | Motor1_PWM_Output_Channel : constant Timer_Channel := Channel_1; 83 | Motor1_Polarity1 : GPIO_Point renames PA10; 84 | Motor1_Polarity2 : GPIO_Point renames PB1; 85 | 86 | Motor2_Encoder_Input1 : GPIO_Point renames PA0; 87 | Motor2_Encoder_Input2 : GPIO_Point renames PA1; 88 | Motor2_Encoder_Timer : constant access Timer := Timer_5'Access; 89 | Motor2_Encoder_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_TIM5_2; 90 | Motor2_PWM_Timer : constant access Timer := Timer_3'Access; 91 | Motor2_PWM_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_TIM3_2; 92 | Motor2_PWM_Output : GPIO_Point renames PB4; 93 | Motor2_PWM_Output_Channel : constant Timer_Channel := Channel_1; 94 | Motor2_Polarity1 : GPIO_Point renames PA2; 95 | Motor2_Polarity2 : GPIO_Point renames PA3; 96 | 97 | -- The hardware used by the remote control AdaFruit BLE USART breakout board 98 | 99 | BLE_UART_MOD_Pin : GPIO_Point renames PD0; 100 | BLE_UART_CTS_Pin : GPIO_Point renames PD3; 101 | BLE_UART_RTS_Pin : GPIO_Point renames PD4; 102 | BLE_UART_RXI_Pin : GPIO_Point renames PD5; -- goes to RXI pin on BLE breakout board 103 | BLE_UART_TXO_Pin : GPIO_Point renames PD6; -- goes to TXO pin on BLE breakout board 104 | 105 | BLE_UART_Transceiver : constant access USART := USART_2'Access; 106 | BLE_UART_Transceiver_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_USART2_7; 107 | BLE_UART_Transceiver_IRQ : constant Ada.Interrupts.Interrupt_ID := USART2_Interrupt; 108 | 109 | end Hardware_Configuration; 110 | -------------------------------------------------------------------------------- /src/hardware_configuration_ir.ads: -------------------------------------------------------------------------------- 1 | ------------------------------------------------------------------------------ 2 | -- -- 3 | -- Copyright (C) 2018, AdaCore -- 4 | -- -- 5 | -- Redistribution and use in source and binary forms, with or without -- 6 | -- modification, are permitted provided that the following conditions are -- 7 | -- met: -- 8 | -- 1. Redistributions of source code must retain the above copyright -- 9 | -- notice, this list of conditions and the following disclaimer. -- 10 | -- 2. Redistributions in binary form must reproduce the above copyright -- 11 | -- notice, this list of conditions and the following disclaimer in -- 12 | -- the documentation and/or other materials provided with the -- 13 | -- distribution. -- 14 | -- 3. Neither the name of STMicroelectronics nor the names of its -- 15 | -- contributors may be used to endorse or promote products derived -- 16 | -- from this software without specific prior written permission. -- 17 | -- -- 18 | -- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -- 19 | -- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -- 20 | -- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -- 21 | -- A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -- 22 | -- HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -- 23 | -- SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -- 24 | -- LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -- 25 | -- DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -- 26 | -- THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -- 27 | -- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -- 28 | -- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -- 29 | -- -- 30 | ------------------------------------------------------------------------------ 31 | 32 | -- This package declares all the hardware devices and related values on the 33 | -- STM32 board actually used by the application. The purpose is to specify 34 | -- them all in one place, so that changes and/or additions can be checked 35 | -- easily for conflicts. 36 | 37 | -- There are multiple versions of this package, depending on the remote 38 | -- receiver communication hardware. For example, there is one package for 39 | -- IR receivers and one for BLE. This is the package for the IR receiver. 40 | 41 | -- Only the remote receiver connections vary among the versions of this 42 | -- package, suggesting that the other values should be in a single, separate 43 | -- package to avoid duplication and thus potential inconsistencies over time. 44 | -- However, the ability to check for device and GPIO conflicts is so valuable 45 | -- as to override the desire to avoid uplication. 46 | 47 | with STM32; use STM32; 48 | with STM32.GPIO; use STM32.GPIO; 49 | with STM32.Timers; use STM32.Timers; 50 | with STM32.I2C; use STM32.I2C; 51 | with STM32.Device; use STM32.Device; 52 | 53 | package Hardware_Configuration is 54 | 55 | -- The hardware on the STM32 board used by the Ultrasonic Sonar sensor (on 56 | -- the NXT_Shield) 57 | 58 | Sonar_Clock_Frequency : constant := 9600; 59 | Sonar_Clock_Pin : GPIO_Point renames PB13; -- SCL 60 | Sonar_Data_Pin : GPIO_Point renames PB11; -- SDA 61 | -- The choice of pins is largely arbitrary because we are bit-banging the 62 | -- I/O instead of using an ob-board I2C device. Nonetheless, the internal 63 | -- pull-up resistor values are not the same across all pins. Specifically, 64 | -- PB10 and PB12 have approximately 11K pull-up resistors, whereas the 65 | -- other pins have approximately 40K pull-up resistors. See table 47 66 | -- "I/O Static Characteristics" in the STM32F405xx STM32F407xx Datasheet. 67 | 68 | -- The hardware on the STM32 board used by the two motors (on the 69 | -- NXT_Shield) 70 | 71 | Motor_PWM_Frequency : constant := 490; 72 | 73 | -- the steering motor (per vehicle.ads and actual wiring) 74 | 75 | Motor1_Encoder_Input1 : GPIO_Point renames PA15; 76 | Motor1_Encoder_Input2 : GPIO_Point renames PB3; 77 | Motor1_Encoder_Timer : constant access Timer := Timer_2'Access; 78 | Motor1_Encoder_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_TIM2_1; 79 | Motor1_PWM_Timer : constant access Timer := Timer_4'Access; 80 | Motor1_PWM_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_TIM4_2; 81 | Motor1_PWM_Output : GPIO_Point renames PB6; 82 | Motor1_PWM_Output_Channel : constant Timer_Channel := Channel_1; 83 | Motor1_Polarity1 : GPIO_Point renames PA10; 84 | Motor1_Polarity2 : GPIO_Point renames PB1; 85 | 86 | -- the engine (per vehicle.ads and actual wiring) 87 | 88 | Motor2_Encoder_Input1 : GPIO_Point renames PA0; 89 | Motor2_Encoder_Input2 : GPIO_Point renames PA1; 90 | Motor2_Encoder_Timer : constant access Timer := Timer_5'Access; 91 | Motor2_Encoder_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_TIM5_2; 92 | Motor2_PWM_Timer : constant access Timer := Timer_3'Access; 93 | Motor2_PWM_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_TIM3_2; 94 | Motor2_PWM_Output : GPIO_Point renames PB4; 95 | Motor2_PWM_Output_Channel : constant Timer_Channel := Channel_1; 96 | Motor2_Polarity1 : GPIO_Point renames PA2; 97 | Motor2_Polarity2 : GPIO_Point renames PA3; 98 | 99 | -- The hardware on the STM32 board used by the remote control IR Receiver 100 | 101 | Receiver_I2C_Port : constant access I2C_Port := I2C_1'Access; 102 | Receiver_I2C_Port_AF : constant STM32.GPIO_Alternate_Function := GPIO_AF_I2C1_4; 103 | Receiver_I2C_Clock_Pin : GPIO_Point renames PB8; -- SCL 104 | Receiver_I2C_Data_Pin : GPIO_Point renames PB9; -- SDA 105 | Lego_NXT_I2C_Frequency : constant := 9600; -- per the Lego HDK 106 | 107 | end Hardware_Configuration; 108 | -------------------------------------------------------------------------------- /src/rc_car.adb: -------------------------------------------------------------------------------- 1 | ------------------------------------------------------------------------------ 2 | -- -- 3 | -- Copyright (C) 2018-2020, AdaCore -- 4 | -- -- 5 | -- Redistribution and use in source and binary forms, with or without -- 6 | -- modification, are permitted provided that the following conditions are -- 7 | -- met: -- 8 | -- 1. Redistributions of source code must retain the above copyright -- 9 | -- notice, this list of conditions and the following disclaimer. -- 10 | -- 2. Redistributions in binary form must reproduce the above copyright -- 11 | -- notice, this list of conditions and the following disclaimer in -- 12 | -- the documentation and/or other materials provided with the -- 13 | -- distribution. -- 14 | -- 3. Neither the name of STMicroelectronics nor the names of its -- 15 | -- contributors may be used to endorse or promote products derived -- 16 | -- from this software without specific prior written permission. -- 17 | -- -- 18 | -- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -- 19 | -- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -- 20 | -- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -- 21 | -- A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -- 22 | -- HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -- 23 | -- SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -- 24 | -- LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -- 25 | -- DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -- 26 | -- THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -- 27 | -- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -- 28 | -- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -- 29 | -- -- 30 | ------------------------------------------------------------------------------ 31 | 32 | -- This program, and the physical car itself, are based on the HiTechnic IR 33 | -- RC Car, with several significant differences. Not least is the fact that no 34 | -- LEGO NXT brick is used, requiring a "cage" to hold the selected MCU card and 35 | -- a place to put the battery. 36 | -- 37 | -- Instructions for building their original car are here: 38 | -- http://www.hitechnic.com/models 39 | -- 40 | -- A video of their car is available on YouTube: 41 | -- https://www.youtube.com/watch?v=KltnZBSvLu4 42 | 43 | -- The computer replacing the Lego Brick is a 32-bit ARM Cortex-M4 MCU on the 44 | -- STM32F4 Discovery card by STMicroelectronics. An FPU is included so we use 45 | -- floating-point in the program. 46 | -- 47 | -- https://www.st.com/en/evaluation-tools/stm32f4discovery.html 48 | 49 | -- In addition, we use a third-party hardware card known as the NXT_Shield to 50 | -- interface to the NXT motors and sonar scanner. Specifically, we use "NXT 51 | -- Shield Version 2" produced by TKJ Electronics. 52 | -- 53 | -- http://blog.tkjelectronics.dk/2011/10/nxt-shield-ver2/ 54 | -- http://shop.tkjelectronics.dk/product_info.php?products_id=29 55 | 56 | -- We use a battery that provides separate connections for +5 and +9 (or +12) 57 | -- volts. The 5V is provided via USB connector, which is precisely what the 58 | -- STM32F4 card requires for power. It isn't light but holds a charge for a 59 | -- long time. The battery is the "XTPower AE-MP-10000-External-Battery" pack. 60 | -- 61 | -- https://www.xtpower.com/ 62 | 63 | -- Available remote controls: 64 | -- 65 | -- Power_Functions_IR_TX_8879 66 | -- see https://www.lego.com/en-us/themes/power-functions/products/ir-speed-remote-control-8879 67 | -- 68 | -- AdaFruit app on cell phone, using BLE for the connection 69 | -- see https://learn.adafruit.com/bluefruit-le-connect/controller 70 | -- 71 | -- 72 | -- The AdaFruit remote app uses a Bluetooth connection from the iPhone or 73 | -- Android mobile phone. The other two remotes use IR, so for those we use the 74 | -- HiTechnic IR receiver. If the AdaFruit BLE is used we must swap out the IR 75 | -- receiver for the Bluefruit LE UART Friend receiver. 76 | -- 77 | -- There is one package declaration for the remote control interface. Each 78 | -- different remote control device above has a corresponding package body 79 | -- implementing the single shared package spec. To have the selected remote be 80 | -- used in the RC_Car program you must build the program with the package body 81 | -- corresponding to the desired controller. This selection is accomplished via 82 | -- the "Remote_Control" scenario variable. 83 | -- 84 | -- See the package bodies for how to use the corresponding remote controls. 85 | 86 | with Last_Chance_Handler; pragma Unreferenced (Last_Chance_Handler); 87 | with Engine_Control; pragma Unreferenced (Engine_Control); 88 | with Steering_Control; pragma Unreferenced (Steering_Control); 89 | with Remote_Control; pragma Unreferenced (Remote_Control); 90 | with Vehicle; 91 | with Global_Initialization; 92 | with System_Configuration; 93 | with STM32.Board; 94 | with Ada.Real_Time; 95 | 96 | procedure RC_Car is 97 | pragma Priority (System_Configuration.Main_Priority); 98 | begin 99 | STM32.Board.Initialize_LEDs; 100 | -- do the above first 101 | 102 | Vehicle.Initialize; 103 | 104 | -- Allow the tasks to start doing their post-initialization work, ie the 105 | -- epoch starts for their periodic loops with the value passed 106 | Global_Initialization.Critical_Instant.Signal (Epoch => Ada.Real_Time.Clock); 107 | 108 | loop 109 | delay until Ada.Real_Time.Time_Last; 110 | end loop; 111 | end RC_Car; 112 | -------------------------------------------------------------------------------- /src/remote_control.ads: -------------------------------------------------------------------------------- 1 | with System_Configuration; 2 | 3 | package Remote_Control 4 | with SPARK_Mode 5 | is 6 | 7 | pragma Elaborate_Body; 8 | 9 | subtype Percentage is Integer range 0 .. 100; 10 | 11 | type Travel_Directions is (Forward, Backward, Neither); 12 | 13 | type Travel_Vector is record 14 | Power : Percentage; 15 | Direction : Travel_Directions; 16 | Emergency_Braking : Boolean := False; 17 | end record 18 | with Size => 32, Atomic; 19 | -- The size is bigger than absolutely necessary but we fit the components 20 | -- to bytes for the sake of efficiency. The main concern is that the size 21 | -- remain such that objects of the record type can be atomically accessed 22 | -- because we are just using shared variables rather than protecting them 23 | -- with a protected object. 24 | 25 | for Travel_Vector use record 26 | Power at 0 range 0 .. 7; 27 | Direction at 0 range 8 .. 15; 28 | Emergency_Braking at 0 range 16 .. 23; 29 | end record; 30 | 31 | function Requested_Vector return Travel_Vector with Inline, Volatile_Function; 32 | 33 | function Requested_Steering_Angle return Integer with Inline, Volatile_Function; 34 | -- The units are angles, positive or negative, relative to the major axis of 35 | -- the vehicle, which is angle zero. 36 | 37 | private 38 | 39 | task Pump with 40 | Storage_Size => 1 * 1024, 41 | Priority => System_Configuration.Remote_Priority; 42 | 43 | end Remote_Control; 44 | -------------------------------------------------------------------------------- /src/remote_control_adafruit_ble_app.adb: -------------------------------------------------------------------------------- 1 | -- This package body implements the remote control package spec using a 2 | -- Bluetooth LE (BLE) breakout board sold by AdaFruit for the communication 3 | -- medium. The controller is a free app also produced by AdaFruit, available 4 | -- for both Android and iOS. The app is named "Bluefruit" since it uses a BLE 5 | -- connection. 6 | -- 7 | -- To use the AdaFruit app as controller, follow these steps *in the order 8 | -- shown* below: 9 | -- 10 | -- 1) First power up the RC car itself 11 | -- 12 | -- 2) Lay the phone down on a flat surface. This step is necessary because we 13 | -- are using data sent from the app to control the drive motor and steering, 14 | -- reflecting the rotations of the phone. When the phone is flat there is no 15 | -- power sent to the drive motor. 16 | -- 17 | -- 3) Rotate your cellphone so that the major axis is in landscape orientation. 18 | -- 19 | -- 4) Start the app and connect to the "Adafruit Bluefruit LE" board listed in 20 | -- the app window. There may be other devices discovered and listed as well, 21 | -- but don't use those. 22 | -- 23 | -- 5) Select the "Controller" option in the app. 24 | -- 25 | -- 6) Slide the control to enable the Accelerometer data. A little window will 26 | -- appear, showing the values. These values are now being sent from the phone 27 | -- to the car. The car is interpreting these values as speed/steering direction 28 | -- controls. 29 | -- 30 | -- 7) Rotate the phone abut the center of the phone, like you're driving with a 31 | -- steering wheel. The car's wheels should turn in response to these rotations. 32 | -- 33 | -- 8) Tip the phone forward or backward to make the wheels run forward or 34 | -- backward. The speed is proportional to the angle tipped. 35 | 36 | with Global_Initialization; 37 | with Ada.Real_Time; use Ada.Real_Time; 38 | with HAL; use HAL; 39 | with Hardware_Configuration; use Hardware_Configuration; 40 | with Serial_IO.Interrupt_Driven; use Serial_IO.Interrupt_Driven; 41 | with BlueFruit_LE_Friend_USART; use BlueFruit_LE_Friend_USART; 42 | with AdaFruit.BLE_Msg_Utils; use AdaFruit.BLE_Msg_Utils; 43 | 44 | package body Remote_Control 45 | with SPARK_Mode 46 | is 47 | 48 | BLE_Port : aliased Serial_Port (BLE_UART_Transceiver_IRQ); 49 | 50 | BLE : Bluefruit_LE_Transceiver (BLE_Port'Access); 51 | 52 | Period : constant Time_Span := Milliseconds (System_Configuration.Remote_Control_Period); 53 | 54 | Current_Vector : Travel_Vector := (0, Forward, Emergency_Braking => False) with 55 | Atomic, Async_Readers, Async_Writers; 56 | 57 | Temp_Vector : Travel_Vector; 58 | 59 | Current_Steering_Target : Integer := 0 with 60 | Atomic, Async_Readers, Async_Writers; 61 | 62 | Temp_Target : Integer; 63 | 64 | procedure Initialize; 65 | -- initialize the BLE receiver 66 | 67 | procedure Receive 68 | (Requested_Vector : out Travel_Vector; 69 | Requested_Steering : out Integer); 70 | -- Get the requested control values from the input device 71 | 72 | ---------------------- 73 | -- Requested_Vector -- 74 | ---------------------- 75 | 76 | function Requested_Vector return Travel_Vector is 77 | begin 78 | return Current_Vector; 79 | end Requested_Vector; 80 | 81 | ------------------------------ 82 | -- Requested_Steering_Angle -- 83 | ------------------------------ 84 | 85 | function Requested_Steering_Angle return Integer is 86 | begin 87 | return Current_Steering_Target; 88 | end Requested_Steering_Angle; 89 | 90 | ---------- 91 | -- Pump -- 92 | ---------- 93 | 94 | task body Pump is 95 | Next_Release : Time; 96 | begin 97 | Global_Initialization.Critical_Instant.Wait (Epoch => Next_Release); 98 | loop 99 | Receive (Temp_Vector, Temp_Target); 100 | Current_Vector := Temp_Vector; 101 | Current_Steering_Target := Temp_Target; 102 | Next_Release := Next_Release + Period; 103 | delay until Next_Release; 104 | end loop; 105 | end Pump; 106 | 107 | ----------------------- 108 | -- Parse_App_Message -- 109 | ----------------------- 110 | 111 | procedure Parse_App_Message is 112 | new Parse_AdaFruit_Controller_Message (Payload => Three_Axes_Data); 113 | 114 | ------------- 115 | -- Receive -- 116 | ------------- 117 | 118 | procedure Receive 119 | (Requested_Vector : out Travel_Vector; 120 | Requested_Steering : out Integer) 121 | is 122 | Buffer_Size : constant := 2 * Accelerometer_Msg_Length; 123 | -- Buffer size is arbitrary but should be bigger than just one message 124 | -- length. An integer multiple of that message length is a good idea. 125 | -- The issue is that we will be receiving a continuous stream of such 126 | -- messages from the phone, and we will not necessarily start receiving 127 | -- just as a new message arrives. We might instead receive a partial 128 | -- message at the beginning of the buffer. Thus when parsing we look for 129 | -- the start of the message, but the point is that we need a big enough 130 | -- buffer to handle at least one message and a partial message too. We 131 | -- don't want the buffer size to be too big, though. 132 | Buffer : String (1 .. Buffer_Size); 133 | TAD : Three_Axes_Data; 134 | Successful_Parse : Boolean; 135 | Power : Integer; 136 | begin 137 | BLE.Get (Buffer); 138 | Parse_App_Message (Buffer, Accelerometer_Msg, TAD, Successful_Parse); 139 | if not Successful_Parse then 140 | Requested_Vector.Emergency_Braking := True; 141 | Requested_Vector.Power := 0; 142 | Requested_Steering := 0; 143 | return; 144 | end if; 145 | 146 | Requested_Steering := Integer (TAD.Y * 100.0); 147 | Power := Integer (-TAD.X * 100.0); 148 | 149 | if Power > 0 then 150 | Requested_Vector.Direction := Forward; 151 | elsif Power < 0 then 152 | Requested_Vector.Direction := Backward; 153 | Power := -Power; -- hence is positive 154 | else -- zero 155 | Requested_Vector.Direction := Neither; 156 | end if; 157 | 158 | Requested_Vector.Power := Integer'Min (100, Power); 159 | -- we don't have a way to signal Emergency braking with the phone... 160 | end Receive; 161 | 162 | ---------------- 163 | -- Initialize -- 164 | ---------------- 165 | 166 | procedure Initialize is 167 | BLE_Friend_Baudrate : constant := 9600; 168 | begin 169 | BLE_Port.Initialize 170 | (Transceiver => BLE_UART_Transceiver, 171 | Transceiver_AF => BLE_UART_Transceiver_AF, 172 | Tx_Pin => BLE_UART_TXO_Pin, 173 | Rx_Pin => BLE_UART_RXI_Pin, 174 | CTS_Pin => BLE_UART_CTS_Pin, 175 | RTS_Pin => BLE_UART_RTS_Pin); 176 | 177 | BLE_Port.Configure (Baud_Rate => BLE_Friend_Baudrate); 178 | BLE_Port.Set_CTS (False); -- essential!! 179 | 180 | BLE.Configure (Mode_Pin => BLE_UART_MOD_Pin); 181 | BLE.Set_Mode (Data); 182 | end Initialize; 183 | 184 | begin 185 | -- initialize the BLE port for receiving messages 186 | Initialize; 187 | end Remote_Control; 188 | -------------------------------------------------------------------------------- /src/remote_control_ir_pf8879.adb: -------------------------------------------------------------------------------- 1 | -- This package body implements the remote control package spec using a Lego 2 | -- infrared receiver and a specific IR control pad using a matching IR 3 | -- transmitter. The control pad is a Lego Power Functions 8879 with two 4 | -- rotary switches, two red buttons (used for stopping), and various other 5 | -- switches. 6 | 7 | -- This remote provides 7 positive and 7 negative non-zero input values per 8 | -- rotary switch, plus zero itself. The absolute values are 0, 16, 30, 44, 58, 9 | -- 72, 86, and 100. Continuing to turn either switch past the point that it 10 | -- presents a max value of 100 has no effect. The software below smooths those 11 | -- direcrete values out somewhat, in a mostly arbitrary manner. 12 | 13 | -- see https://www.lego.com/en-us/themes/power-functions/products/ir-speed-remote-control-8879 14 | 15 | -- NOTE THAT THE ROTARY SWITCHES DO NOT GO BACK TO ZERO WHEN RELEASED, THEY 16 | -- MUST BE MANUALLY DIALED BACK DOWN. That's just the way the remote works. 17 | -- The car will require this re-zeroing to be done manually, on startup, if the 18 | -- switches are not at zero when starting. In this case the blue LED will be 19 | -- lit when the steering switch is non-zero, and the red LED will be lit when 20 | -- the power switch is non-zero. One or both may be lit. 21 | 22 | -- The rotary switch on the right is for controlling the engine, ie the motion 23 | -- of the vehicle itself. Turning it to the right will cause the vehicle to 24 | -- go forward, to the left will cause backward travel. The speed will increase 25 | -- as the switch is turned further in the same direction. Speed will decrease 26 | -- if the switch is turned in the opposite direction, and will continue 27 | -- to decrease until it goes to zero and then starts going in the oposite 28 | -- direction entirely. 29 | 30 | -- The rotary switch on the left controls the steering direction. Turning to 31 | -- the right steers the vehicle to the right, the reverse for turning to the 32 | -- left. The switch behaves like the engine power switch on the right, in 33 | -- that turning in the opposite direction reduces the degree of turn currently 34 | -- applied. 35 | 36 | -- Pressing the red button below the switch on the right will cause the vehicle 37 | -- to stop immediately. Although the car will stop, the controls will not go to 38 | -- zero on the remote automatically. Therefore the on-board program will force 39 | -- the user to manually zero them before resuming, as described above. 40 | 41 | -- In addition to connections for power (+5V) and ground, the HiTechnic IR 42 | -- receiver also has connections at PB8 (yellow wire) and PB9 (green wire). 43 | 44 | with Global_Initialization; 45 | with Ada.Real_Time; use Ada.Real_Time; 46 | with STM32.Board; use STM32.Board; 47 | with HAL; use HAL; 48 | with Hardware_Configuration; 49 | with HiTechnic.IR_Receivers; use HiTechnic.IR_Receivers; 50 | with NXT.Digital; use NXT.Digital; 51 | 52 | package body Remote_Control 53 | with SPARK_Mode 54 | is 55 | 56 | IR_Sensor_Address : constant I2C_Device_Address := 1; -- unshifted! 57 | 58 | Receiver : IR_Receiver (Device_Hardware_Address => IR_Sensor_Address); 59 | -- The HiTechnic sensor receiving data from the LEGO IR remote 60 | 61 | Steering_LED : User_LED renames Orange_LED; -- arbitrary 62 | Engine_LED : User_LED renames Blue_LED; -- arbitrary 63 | 64 | Brake_Button_Pushed : constant := -128; 65 | -- For the right-hand side red button on the PF8879 Remote Control. 66 | 67 | Period : constant Time_Span := Milliseconds (System_Configuration.Remote_Control_Period); 68 | 69 | Initial_Steering_Angle : constant := 0; 70 | 71 | procedure Zero_Remote_Rotary_Switches; 72 | -- Interact with human driver so that the 8879 rotary inputs are at the zero 73 | -- position. On this remote, the left and right switches used for steering 74 | -- and power control will be at whatever value they were when the remote 75 | -- was last used. The control software must not use those inputs because the 76 | -- car could immediately turn and move unexpectedly, instead of being still. 77 | -- There is no physical indicator on the remote to show what position they 78 | -- are in, so the user cannot simply dial back to zero before powering up 79 | -- the car. Thus we blink the two LEDs until the received inputs are zero. 80 | 81 | procedure Initialize; 82 | -- init the hardware 83 | 84 | procedure Receive 85 | (Requested_Power : in out Percentage; 86 | Requested_Direction : in out Travel_Directions; 87 | Emergency_Braking : in out Boolean; 88 | Requested_Steering : in out Integer); 89 | -- Get the requested control values from the IR Receiver sensor 90 | 91 | -- This remote provides 7 positive and 7 negative non-zero input values per 92 | -- rotary switch, plus zero itself. The absolute values are 0, 16, 30, 44, 93 | -- 58, 72, 86, and 100. 94 | 95 | function Mapped_Angle (Requested : Integer) return Integer; 96 | -- Map the the non-continuous steering inputs from the remote to smoothed 97 | -- turn angles 98 | 99 | function Mapped_Power (Requested : Percentage) return Percentage; 100 | -- Map the non-continuous power inputs received from the remote to smoothed 101 | -- power settings 102 | 103 | Current_Steering_Target : Integer := Initial_Steering_Angle with 104 | Atomic, Async_Readers, Async_Writers; 105 | 106 | Current_Vector : Travel_Vector := Travel_Vector'(0, Forward, Emergency_Braking => False) with 107 | Atomic, Async_Readers, Async_Writers; 108 | 109 | Temp : Travel_Vector; 110 | 111 | ---------------------- 112 | -- Requested_Vector -- 113 | ---------------------- 114 | 115 | function Requested_Vector return Travel_Vector is 116 | begin 117 | return Current_Vector; 118 | end Requested_Vector; 119 | 120 | ------------------------------ 121 | -- Requested_Steering_Angle -- 122 | ------------------------------ 123 | 124 | function Requested_Steering_Angle return Integer is 125 | begin 126 | return Current_Steering_Target; 127 | end Requested_Steering_Angle; 128 | 129 | ---------- 130 | -- Pump -- 131 | ---------- 132 | 133 | task body Pump is 134 | Next_Release : Time; 135 | Requested_Steering : Integer := 0; 136 | Requested_Power : Percentage := 0; 137 | Requested_Direction : Travel_Directions := Forward; 138 | Requested_Braking : Boolean := False; 139 | begin 140 | Global_Initialization.Critical_Instant.Wait (Epoch => Next_Release); 141 | loop 142 | Receive (Requested_Power, 143 | Requested_Direction, 144 | Requested_Braking, 145 | Requested_Steering); 146 | 147 | Current_Steering_Target := Mapped_Angle (Requested_Steering); 148 | 149 | Temp.Power := Mapped_Power (Requested_Power); 150 | Temp.Direction := Requested_Direction; 151 | Temp.Emergency_Braking := Requested_Braking; 152 | Current_Vector := Temp; 153 | 154 | if Requested_Braking then 155 | -- The car will come to an immediate stop as a result of the 156 | -- braking but the controls will not go to zero on the remote. 157 | -- Therefore we must wait for the user to manually zero them 158 | -- before resuming. 159 | Zero_Remote_Rotary_Switches; 160 | end if; 161 | 162 | Next_Release := Next_Release + Period; 163 | delay until Next_Release; 164 | end loop; 165 | end Pump; 166 | 167 | ------------- 168 | -- Receive -- 169 | ------------- 170 | 171 | procedure Receive 172 | (Requested_Power : in out Percentage; 173 | Requested_Direction : in out Travel_Directions; 174 | Emergency_Braking : in out Boolean; 175 | Requested_Steering : in out Integer) 176 | is 177 | Switches : Raw_Sensor_Data; 178 | Steering_Switches : Raw_Sensor_Values renames Switches.A; 179 | Engine_Switches : Raw_Sensor_Values renames Switches.B; 180 | IO_Successful : Boolean; 181 | Power : Integer; 182 | begin 183 | Receiver.Get_Raw_Data (Switches, IO_Successful); 184 | if not IO_Successful then 185 | Blue_LED.Set; 186 | Emergency_Braking := True; 187 | return; 188 | end if; 189 | 190 | -- Compute the engine power. Only one of Engine_Switches will be nonzero 191 | -- (assuming only one control used) so we can simpy add them all up without 192 | -- trying to find which one it is. 193 | Power := Integer (Engine_Switches (1)) + 194 | Integer (Engine_Switches (2)) + 195 | Integer (Engine_Switches (3)) + 196 | Integer (Engine_Switches (4)); 197 | 198 | Emergency_Braking := False; 199 | Requested_Direction := Neither; 200 | if Power = Brake_Button_Pushed then 201 | Emergency_Braking := True; 202 | return; 203 | elsif Power > 0 then 204 | Requested_Direction := Forward; 205 | elsif Power < 0 then 206 | Requested_Direction := Backward; 207 | Power := -Power; -- hence power is positive 208 | end if; 209 | Requested_Power := Integer'Min (100, Power); 210 | 211 | -- Compute the steering target angle. Only one of Steering_Switches will 212 | -- be nonzero (assuming only one control used). 213 | Requested_Steering := Integer (Steering_Switches (1)) + 214 | Integer (Steering_Switches (2)) + 215 | Integer (Steering_Switches (3)) + 216 | Integer (Steering_Switches (4)); 217 | end Receive; 218 | 219 | ---------------- 220 | -- Initialize -- 221 | ---------------- 222 | 223 | procedure Initialize is 224 | pragma SPARK_Mode (Off); 225 | use Hardware_Configuration; 226 | begin 227 | Receiver.Configure 228 | (Port => Receiver_I2C_Port, 229 | SCL => Receiver_I2C_Clock_Pin, 230 | SDA => Receiver_I2C_Data_Pin, 231 | AF_Code => Receiver_I2C_Port_AF, 232 | Clock_Speed => Lego_NXT_I2C_Frequency); 233 | 234 | Zero_Remote_Rotary_Switches; 235 | end Initialize; 236 | 237 | ------------------ 238 | -- Mapped_Angle -- 239 | ------------------ 240 | 241 | function Mapped_Angle (Requested : Integer) return Integer is 242 | Negative : constant Boolean := Requested < 0; 243 | Result : Integer; 244 | begin 245 | -- Map the received steering angle from the remote to a smoother set of 246 | -- values. These Result values are arbitrary but intended to make the 247 | -- steering responsive. Note that the steering control computer will 248 | -- limit the input to whatever maximum angle is physically possible. 249 | case abs (Requested) is 250 | when 0 => Result := 0; 251 | when 16 => Result := 5; 252 | when 30 => Result := 9; 253 | when 44 => Result := 15; 254 | when 58 => Result := 20; 255 | when 72 => Result := 25; 256 | when 86 => Result := 30; 257 | when 100 => Result := 40; 258 | when others => 259 | -- shouldn't be possible, but don't trust the hardware... 260 | Result := 0; 261 | end case; 262 | Result := (if Negative then -Result else Result); 263 | return Result; 264 | end Mapped_Angle; 265 | 266 | ------------------ 267 | -- Mapped_Power -- 268 | ------------------ 269 | 270 | function Mapped_Power (Requested : Percentage) return Percentage is 271 | Result : Percentage; 272 | begin 273 | -- Map the received power selection from the remote to a smoother set of 274 | -- values. 275 | case Requested is 276 | when 0 => Result := 0; 277 | when 16 => Result := 30; 278 | when 30 => Result := 40; 279 | when 44 => Result := 50; 280 | when 58 => Result := 60; 281 | when 72 => Result := 70; 282 | when 86 => Result := 80; 283 | when 100 => Result := 100; 284 | when others => 285 | -- shouldn't happen, but don't trust the hardware... 286 | Result := 0; 287 | end case; 288 | return Result; 289 | end Mapped_Power; 290 | 291 | --------------------------------- 292 | -- Zero_Remote_Rotary_Switches -- 293 | --------------------------------- 294 | 295 | procedure Zero_Remote_Rotary_Switches is 296 | Switches : Raw_Sensor_Data; 297 | Steering_Switches : Raw_Sensor_Values renames Switches.A; 298 | Engine_Switches : Raw_Sensor_Values renames Switches.B; 299 | 300 | IO_Successful : Boolean; 301 | IO_Failure_Count : Natural := 0; 302 | Max_IO_Failures : constant := 5; -- arbitrary 303 | 304 | Steering_Input : Integer; 305 | Power_Input : Integer; 306 | 307 | Blink_Rate : constant Time_Span := Milliseconds (200); -- arbitrary 308 | Next_Release : Time; 309 | begin 310 | All_LEDs_Off; 311 | Next_Release := Clock; 312 | loop 313 | Receiver.Get_Raw_Data (Switches, IO_Successful); 314 | if not IO_Successful then 315 | IO_Failure_Count := IO_Failure_Count + 1; 316 | if IO_Failure_Count = Max_IO_Failures then 317 | raise Program_Error with "IR sensor failure"; 318 | end if; 319 | else 320 | -- The user rotates the switches until they both reach the zero 321 | -- points. We don't care which channel the user is using, so we 322 | -- just add them up. 323 | Power_Input := Integer (Engine_Switches (1)) + 324 | Integer (Engine_Switches (2)) + 325 | Integer (Engine_Switches (3)) + 326 | Integer (Engine_Switches (4)); 327 | 328 | Steering_Input := Integer (Steering_Switches (1)) + 329 | Integer (Steering_Switches (2)) + 330 | Integer (Steering_Switches (3)) + 331 | Integer (Steering_Switches (4)); 332 | 333 | -- indicate to the user which switches need to go to zero 334 | if Steering_Input /= 0 then 335 | Steering_LED.Set; 336 | else 337 | Steering_LED.Clear; 338 | end if; 339 | if Power_Input /= 0 then 340 | Engine_LED.Set; 341 | else 342 | Engine_LED.Clear; 343 | end if; 344 | end if; 345 | 346 | exit when Steering_Input = 0 and Power_Input = 0; 347 | 348 | Next_Release := Next_Release + Blink_Rate; 349 | delay until Next_Release; 350 | end loop; 351 | end Zero_Remote_Rotary_Switches; 352 | 353 | begin 354 | Initialize; 355 | end Remote_Control; 356 | -------------------------------------------------------------------------------- /src/steering_control.adb: -------------------------------------------------------------------------------- 1 | with Global_Initialization; 2 | with Ada.Real_Time; use Ada.Real_Time; 3 | with Vehicle; use Vehicle; 4 | with NXT.Motors; use NXT.Motors; 5 | with Remote_Control; 6 | with Math_Utilities; 7 | with Process_Control_Floating_Point; 8 | 9 | package body Steering_Control 10 | with SPARK_Mode 11 | is 12 | 13 | Period : constant Time_Span := Milliseconds (System_Configuration.Steering_Control_Period); 14 | -- NB: important to PID tuning! 15 | 16 | package Closed_Loop is new Process_Control_Floating_Point (Float, Long_Float); 17 | use Closed_Loop; 18 | 19 | -- steering PID gain constants 20 | Kp : constant := 6.0; 21 | Ki : constant := 5.0; 22 | Kd : constant := 0.1; 23 | 24 | Power_Level_Last : constant Float := Float (Power_Level'Last); 25 | 26 | Power_Level_Limits : constant Closed_Loop.Bounds := 27 | (Min => -Power_Level_Last, Max => +Power_Level_Last); 28 | -- The limits for the PID controller output power values, based on the 29 | -- NXT motor's largest power value. The NXT Power_Level type is an integer 30 | -- ranging from 0 to 100. The PID controller will compute negative values 31 | -- as well as positive values, to turn the steering mechanism in either 32 | -- direction, so we want to limit the PID to -100 .. 100. We later take the 33 | -- absolute value after using the sign to get the direction, in procedure 34 | -- Convert_To_Motor_Values. 35 | 36 | Encoder_Counts_Per_Degree : constant Float := Float (NXT.Motors.Encoder_Counts_Per_Revolution) / 360.0; 37 | 38 | procedure Limit is new Math_Utilities.Bound_Floating_Value (Float); 39 | 40 | procedure Initialize_Steering_Mechanism (Center_Offset : out Float) with 41 | Post => Center_Offset > 0.0; 42 | -- Compute the steering zero offset value by powering the steering mechanism 43 | -- to the mechanical limits. 44 | 45 | function Current_Motor_Angle (This : Basic_Motor) return Float with Inline; 46 | -- Returns the current encoder count for This motor in degree units 47 | 48 | procedure Convert_To_Motor_Values 49 | (Signed_Power : Float; 50 | Motor_Power : out NXT.Motors.Power_Level; 51 | Direction : out NXT.Motors.Directions) 52 | with 53 | Inline, 54 | Pre => Within_Limits (Signed_Power, Power_Level_Limits); 55 | -- Convert the signed power value from the PID controller to NXT motor 56 | -- values. We know the precondition will hold because we set those limits 57 | -- via the call to Steering_Computer.Configure 58 | 59 | ----------- 60 | -- Servo -- 61 | ----------- 62 | 63 | task body Servo is 64 | Next_Release : Time; 65 | Target_Angle : Float; 66 | Current_Angle : Float := 0.0; -- zero for call to Steering_Computer.Enable 67 | Steering_Power : Float := 0.0; -- zero for call to Steering_Computer.Enable 68 | Motor_Power : NXT.Motors.Power_Level; 69 | Rotation_Direction : NXT.Motors.Directions; 70 | Steering_Offset : Float; 71 | Steering_Computer : PID_Controller := Configured_Controller 72 | (Proportional_Gain => Kp, 73 | Integral_Gain => Ki, 74 | Derivative_Gain => Kd, 75 | Invocation_Period => System_Configuration.Steering_Control_Period, 76 | Output_Limits => Power_Level_Limits, 77 | Direction => Closed_Loop.Direct); 78 | begin 79 | Global_Initialization.Critical_Instant.Wait (Epoch => Next_Release); 80 | 81 | Initialize_Steering_Mechanism (Steering_Offset); 82 | 83 | Steering_Computer.Enable (Process_Variable => Current_Angle, Control_Variable => Steering_Power); 84 | loop 85 | pragma Loop_Invariant (Steering_Computer.Current_Output_Limits = Power_Level_Limits); 86 | pragma Loop_Invariant (Within_Limits (Steering_Power, Power_Level_Limits)); 87 | 88 | Current_Angle := Current_Motor_Angle (Steering_Motor) - Steering_Offset; 89 | 90 | Target_Angle := Float (Remote_Control.Requested_Steering_Angle); 91 | Limit (Target_Angle, -Steering_Offset, Steering_Offset); 92 | 93 | Steering_Computer.Compute_Output 94 | (Process_Variable => Current_Angle, 95 | Setpoint => Target_Angle, 96 | Control_Variable => Steering_Power); 97 | 98 | Convert_To_Motor_Values (Steering_Power, Motor_Power, Rotation_Direction); 99 | 100 | Steering_Motor.Engage (Rotation_Direction, Motor_Power); 101 | 102 | Next_Release := Next_Release + Period; 103 | delay until Next_Release; 104 | end loop; 105 | end Servo; 106 | 107 | ----------------------------- 108 | -- Convert_To_Motor_Values -- 109 | ----------------------------- 110 | 111 | procedure Convert_To_Motor_Values 112 | (Signed_Power : Float; 113 | Motor_Power : out NXT.Motors.Power_Level; 114 | Direction : out NXT.Motors.Directions) 115 | is 116 | begin 117 | Direction := Vehicle.To_Steering_Motor_Direction (Signed_Power); 118 | -- The motor values are a percentage from 0 .. 100. The sign of the value 119 | -- is only used in the statement above, for "turn" directions. 120 | Motor_Power := Power_Level (abs (Signed_Power)); 121 | end Convert_To_Motor_Values; 122 | 123 | ------------------------- 124 | -- Current_Motor_Angle -- 125 | ------------------------- 126 | 127 | function Current_Motor_Angle (This : Basic_Motor) return Float is 128 | ((Float (This.Encoder_Count) / Encoder_Counts_Per_Degree)); 129 | 130 | ----------------------------------- 131 | -- Initialize_Steering_Mechanism -- 132 | ----------------------------------- 133 | 134 | procedure Initialize_Steering_Mechanism (Center_Offset : out Float) is 135 | Current_Angle : Float; 136 | Previous_Angle : Float; 137 | Sample_Inteval : constant Time_Span := Milliseconds (100); -- arbitrary 138 | Next_Release : Time; 139 | Testing_Power : constant Power_Level := 80; 140 | -- The power setting is arbitrary, but should be kept relatively low so 141 | -- as to avoid stressing the steering mechanism unduly. That said, it 142 | -- must be enough to really go to the physical limits, even on carpet. 143 | begin 144 | -- The purpose of this routine is to determine the value of the global 145 | -- mechanical steering "zero," ie, the value when steering straight 146 | -- ahead. This value is used as an offset when steering to commanded 147 | -- angles received from the remote control. Therefore, we drive the 148 | -- steering mechanism all the way to the maximums for left and right, 149 | -- then compute half of that angle for the center steering offset. 150 | 151 | -- steer to right-most mechanical limit 152 | Current_Angle := Current_Motor_Angle (Steering_Motor); 153 | Steering_Motor.Engage (To_The_Right, Power => Testing_Power); 154 | Next_Release := Clock; 155 | loop 156 | -- give the motor time to move 157 | Next_Release := Next_Release + Sample_Inteval; 158 | delay until Next_Release; 159 | 160 | Previous_Angle := Current_Angle; 161 | Current_Angle := Current_Motor_Angle (Steering_Motor); 162 | -- Exit when no further progress made. We are driving the motor 163 | -- backward so the encoder count will be decreasing, hence "<" 164 | exit when not (Current_Angle < Previous_Angle); 165 | end loop; 166 | 167 | -- Now that we are at the right-most limit, we reset the motor encoder 168 | -- count to zero before driving the motor to the left-most limit. As 169 | -- a result, the value read at the left-most limit will give us the 170 | -- full angle range possible for steering. 171 | Steering_Motor.Reset_Encoder_Count; 172 | 173 | -- steer to left-most mechanical limit 174 | Current_Angle := Current_Motor_Angle (Steering_Motor); 175 | Steering_Motor.Engage (To_The_Left, Power => Testing_Power); 176 | Next_Release := Clock; 177 | loop 178 | -- give the motor time to move 179 | Next_Release := Next_Release + Sample_Inteval; 180 | delay until Next_Release; 181 | 182 | Previous_Angle := Current_Angle; 183 | Current_Angle := Current_Motor_Angle (Steering_Motor); 184 | -- Exit when no further progress made. We are driving the motor 185 | -- forward so the encoder count will be increasing, hence ">" 186 | exit when not (Current_Angle > Previous_Angle); 187 | end loop; 188 | 189 | -- Current_Angle is now the maximum steering angle mechanically possible, 190 | -- from far right to far left. Therefore the center is half that angle. 191 | Center_Offset := Current_Angle / 2.0; 192 | 193 | -- Because our "To_The_Left" is "Forward" to the NXT motor direction, the 194 | -- encoder values will be increasing away from zero. 195 | pragma Assert (To_The_Left = Forward); -- make sure... 196 | -- The encoder was reset before moving to the left-most limit, therefore 197 | -- Center_Offset is positive. 198 | pragma Assume (Center_Offset > 0.0); 199 | -- NB: if the above assertion fails during debugging, make sure that 200 | -- the battery is supplying power to the motors etc so that the steering 201 | -- motor actually moves. 202 | 203 | -- Rationale for this procedure, in light of the above: 204 | -- 205 | -- The steering control computer receives steering angle requests 206 | -- from the user via the remote control. These requests use a frame 207 | -- of reference oriented on the major axis of the vehicle. Because we 208 | -- use the steering motor rotation angle to steer the vehicle, we must 209 | -- translate the requests from the user's frame of reference (ie, the 210 | -- vehicle's) into the frame of reference of the steering motor. The 211 | -- steering motor's frame of reference is defined by the steering 212 | -- mechanism's physical connection to the motor shaft when the shaft 213 | -- rotation encoder count is zero. 214 | -- 215 | -- Therefore, to do the translation we set the motor encoder to zero at 216 | -- some known point relative to the vehicle's major axis and then handle 217 | -- the difference between that motor "zero" and the "zero" corresponding 218 | -- to the major axis. We thus orient the motor's frame of reference to 219 | -- that of the vehicle. 220 | -- 221 | -- Ideally, the two frames of reference would be aligned (both zero) on 222 | -- the vehicle's major axis, but in the software there's no way to know 223 | -- where that major axis is. This is a matter of physical reality. 224 | -- 225 | -- So, how can the software identify some known point on the vehicle (to 226 | -- set the motor encoder to zero there)? 227 | -- 228 | -- We know how the steering mechanism is physically oriented to the 229 | -- vehicle's major axis: the angle for steering "straight ahead" 230 | -- corresponds with the axis. We also know (or can safely assume) that 231 | -- the steering mechanism is symmetric, ie, it can steer equally in 232 | -- either direction. Therefore, the center of the arc described by the 233 | -- steering mechanism is "straight ahead" and is, therefore, aligned 234 | -- with the major axis of the vehicle. 235 | -- 236 | -- We (the software) can measure the steering arc because we can locate 237 | -- the mechanical far-left and far-right steering mechanism extents. 238 | -- 239 | -- In addition, those mechanical extents are known points on the 240 | -- vehicle relative to the major axis so we can set the motor's frame of 241 | -- reference to one of them. It doesn't matter which one we use. In the 242 | -- code above, we had chosen, arbitrarily, to measure from far-right to 243 | -- far-left, so we had set the encoder to zero at the far-right extent. 244 | -- Therefore, we simply use the far-right extent to set the motor's frame 245 | -- of reference. 246 | -- 247 | -- Now the difference between the two frames of reference is simply the 248 | -- difference between the far-right steering extent and the center of the 249 | -- steering arc. That value is precisely half the total number of degrees 250 | -- in the arc. Therefore, the steering computer will subtract that value 251 | -- from the motor's reported angle to translate the reported value into 252 | -- the vehicle's frame of reference. 253 | end Initialize_Steering_Mechanism; 254 | 255 | end Steering_Control; 256 | -------------------------------------------------------------------------------- /src/steering_control.ads: -------------------------------------------------------------------------------- 1 | with System_Configuration; 2 | 3 | package Steering_Control 4 | with SPARK_Mode 5 | is 6 | 7 | pragma Elaborate_Body; 8 | 9 | private 10 | 11 | task Servo with 12 | Storage_Size => 1 * 1024, 13 | Priority => System_Configuration.Steering_Priority; 14 | 15 | end Steering_Control; 16 | -------------------------------------------------------------------------------- /src/system_configuration.ads: -------------------------------------------------------------------------------- 1 | -- Task periods and priorities 2 | 3 | with System; use System; 4 | 5 | package System_Configuration is 6 | 7 | -- These constants are the priorities of the tasks in the system, defined 8 | -- here for ease of setting with the big picture in view. 9 | 10 | Main_Priority : constant Priority := Priority'First; -- ie lowest 11 | Engine_Monitor_Priority : constant Priority := Main_Priority + 1; 12 | Remote_Priority : constant Priority := Engine_Monitor_Priority + 1; 13 | Engine_Control_Priority : constant Priority := Remote_Priority + 1; 14 | Steering_Priority : constant Priority := Engine_Control_Priority + 1; 15 | 16 | -- The engine control priority needs high priority because it calls the 17 | -- sonar sensor via the collision detection module, and that sonar object 18 | -- uses bit-banged I/O (unfortunately) and so should not be preempted much. 19 | -- Ugh. I'd rather do the response-time analysis and set the priorities by 20 | -- period... 21 | 22 | Highest_Priority : Priority renames Steering_Priority; 23 | -- Whichever is highest. All the tasks call into the global initialization 24 | -- PO to await completion before doing anything interesting, so the PO 25 | -- requires the highest of those caller priorities. 26 | 27 | -- These constants are the tasks' periods, defined here for ease of setting 28 | -- with the big picture in view. They are merely named numbers rather than 29 | -- values of type Time_Span because their values are also used for other 30 | -- purposes, eg configuring the steering PID controller. 31 | 32 | -- TODO: align priorities with periods! 33 | 34 | Engine_Control_Period : constant := 200; -- milliseconds 35 | Engine_Monitor_Period : constant := 150; -- milliseconds 36 | Remote_Control_Period : constant := 100; -- milliseconds 37 | Steering_Control_Period : constant := 10; -- milliseconds 38 | -- NB: important to PID tuning! 39 | 40 | end System_Configuration; 41 | -------------------------------------------------------------------------------- /src/vehicle.adb: -------------------------------------------------------------------------------- 1 | with Global_Initialization; 2 | with Ada.Numerics; use Ada.Numerics; 3 | with Ada.Real_Time; use Ada.Real_Time; 4 | with Hardware_Configuration; 5 | with Recursive_Moving_Average_Filters_Discretes; 6 | 7 | package body Vehicle with 8 | SPARK_Mode, 9 | Refined_State => (Internal_State => (Engine_Monitor, Current_Speed, Total_Distance_Traveled)) 10 | is 11 | 12 | Period : constant Time_Span := Milliseconds (System_Configuration.Engine_Monitor_Period); 13 | Sample_Interval : constant Float := Float (System_Configuration.Engine_Monitor_Period) / 1000.0; 14 | -- The time interval at which the task samples the encoder counts, which is 15 | -- also the period of the task itself. 16 | 17 | subtype Nonnegative_Float is Float range 0.0 .. Float'Last; 18 | 19 | Counts_Per_Revolution : constant Nonnegative_Float := Float (Encoder_Counts_Per_Revolution); 20 | Wheel_Circumference : constant Nonnegative_Float := Pi * Wheel_Diameter * Gear_Ratio; 21 | Distance_Per_Encoder_Count : constant Nonnegative_Float := Wheel_Circumference / Counts_Per_Revolution; 22 | 23 | Current_Speed : Nonnegative_Float := 0.0 with Atomic, Async_Readers, Async_Writers; 24 | -- in cm/sec 25 | -- assigned by the Engine_Monitor task 26 | 27 | Total_Distance_Traveled : Nonnegative_Float := 0.0 with Atomic, Async_Readers, Async_Writers; 28 | -- in cm 29 | -- assigned by the Engine_Monitor task 30 | 31 | procedure Initialize_Motors (Steering, Locomotion : out Basic_Motor); 32 | -- Initializes the two NXT motors on the NXT_Shield. This must be done 33 | -- before any software commands or accesses to those motors. 34 | 35 | procedure Initialize_Ultrasonic (Sensor : out Ultrasonic_Sonar_Sensor) with 36 | Post => Configured (Sensor) and then 37 | Enabled (Sensor) and then 38 | Current_Scan_Mode (Sensor) = Continuous; 39 | -- Initializes the Sonar on the NXT_Shield. This must be done 40 | -- before any software commands or accesses to the sensor. 41 | 42 | function Safely_Subtract (Left, Right : Motor_Encoder_Counts) return Motor_Encoder_Counts; 43 | -- Computes Left - Right without actually overflowing. The result is either 44 | -- the subtracted value, or, if the subtraction would overflow, the 'First 45 | -- or 'Last for type Motor_Encoder_Counts. 46 | 47 | ----------- 48 | -- Speed -- 49 | ----------- 50 | 51 | function Speed return Float is 52 | begin 53 | return Current_Speed; 54 | end Speed; 55 | 56 | -------------- 57 | -- Odometer -- 58 | -------------- 59 | 60 | function Odometer return Float is 61 | begin 62 | return Total_Distance_Traveled; 63 | end Odometer; 64 | 65 | --------------------------------- 66 | -- To_Steering_Motor_Direction -- 67 | --------------------------------- 68 | 69 | function To_Steering_Motor_Direction (Power : Float) return NXT.Motors.Directions is 70 | (if Power < 0.0 then To_The_Right else To_The_Left); 71 | 72 | ----------------------------------- 73 | -- To_Propulsion_Motor_Direction -- 74 | ----------------------------------- 75 | 76 | function To_Propulsion_Motor_Direction (Direction : Remote_Control.Travel_Directions) 77 | return NXT.Motors.Directions 78 | is (if Direction = Remote_Control.Forward then NXT.Motors.Forward else NXT.Motors.Backward); 79 | 80 | ------------------- 81 | -- Encoder_Noise -- 82 | ------------------- 83 | 84 | package Encoder_Noise is new Recursive_Moving_Average_Filters_Discretes 85 | (Sample => Motor_Encoder_Counts, 86 | Accumulator => Long_Long_Integer); 87 | 88 | -------------------- 89 | -- Engine_Monitor -- 90 | -------------------- 91 | 92 | task body Engine_Monitor is 93 | Next_Release : Time; 94 | Current_Count : Motor_Encoder_Counts := 0; 95 | Previous_Count : Motor_Encoder_Counts; 96 | Encoder_Delta : Motor_Encoder_Counts; 97 | Interval_Distance : Nonnegative_Float; 98 | Current_Distance : Nonnegative_Float; 99 | Noise_Filter : Encoder_Noise.RMA_Filter (Window_Size => 5); -- arbitrary size 100 | begin 101 | Noise_Filter.Reset; 102 | Global_Initialization.Critical_Instant.Wait (Epoch => Next_Release); 103 | loop 104 | Previous_Count := Current_Count; 105 | Noise_Filter.Insert (Engine.Encoder_Count); 106 | Current_Count := Noise_Filter.Value; 107 | 108 | Encoder_Delta := Safely_Subtract (Current_Count, Previous_Count); 109 | 110 | Interval_Distance := abs (Float (Encoder_Delta) * Distance_Per_Encoder_Count); 111 | Current_Speed := Interval_Distance / Sample_Interval; -- package global variable 112 | 113 | Current_Distance := Total_Distance_Traveled; 114 | Current_Distance := Current_Distance + Interval_Distance; 115 | Total_Distance_Traveled := Current_Distance; -- package global variable 116 | 117 | Next_Release := Next_Release + Period; 118 | delay until Next_Release; 119 | end loop; 120 | end Engine_Monitor; 121 | 122 | --------------------- 123 | -- Safely_Subtract -- 124 | --------------------- 125 | 126 | function Safely_Subtract 127 | (Left, Right : Motor_Encoder_Counts) 128 | return Motor_Encoder_Counts 129 | is 130 | Result : Motor_Encoder_Counts; 131 | begin 132 | if Right > 0 then 133 | if Left >= Motor_Encoder_Counts'First + Right then 134 | Result := Left - Right; 135 | else -- would overflow 136 | Result := Motor_Encoder_Counts'First; 137 | end if; 138 | else -- Right is negative or zero 139 | if Left <= Motor_Encoder_Counts'Last + Right then 140 | Result := Left - Right; 141 | else -- would overflow 142 | Result := Motor_Encoder_Counts'Last; 143 | end if; 144 | end if; 145 | return Result; 146 | end Safely_Subtract; 147 | 148 | ----------------------- 149 | -- Initialize_Motors -- 150 | ----------------------- 151 | 152 | procedure Initialize_Motors (Steering, Locomotion : out Basic_Motor) is 153 | pragma SPARK_Mode (Off); 154 | use Hardware_Configuration; 155 | begin 156 | Steering.Initialize 157 | (Encoder_Input1 => Motor1_Encoder_Input1, 158 | Encoder_Input2 => Motor1_Encoder_Input2, 159 | Encoder_Timer => Motor1_Encoder_Timer, 160 | Encoder_AF => Motor1_Encoder_AF, 161 | PWM_Timer => Motor1_PWM_Timer, 162 | PWM_Output_Frequency => Motor_PWM_Frequency, 163 | PWM_AF => Motor1_PWM_AF, 164 | PWM_Output => Motor1_PWM_Output, 165 | PWM_Output_Channel => Motor1_PWM_Output_Channel, 166 | Polarity1 => Motor1_Polarity1, 167 | Polarity2 => Motor1_Polarity2); 168 | 169 | Locomotion.Initialize 170 | (Encoder_Input1 => Motor2_Encoder_Input1, 171 | Encoder_Input2 => Motor2_Encoder_Input2, 172 | Encoder_Timer => Motor2_Encoder_Timer, 173 | Encoder_AF => Motor2_Encoder_AF, 174 | PWM_Timer => Motor2_PWM_Timer, 175 | PWM_Output_Frequency => Motor_PWM_Frequency, 176 | PWM_AF => Motor2_PWM_AF, 177 | PWM_Output => Motor2_PWM_Output, 178 | PWM_Output_Channel => Motor2_PWM_Output_Channel, 179 | Polarity1 => Motor2_Polarity1, 180 | Polarity2 => Motor2_Polarity2); 181 | end Initialize_Motors; 182 | 183 | --------------------------- 184 | -- Initialize_Ultrasonic -- 185 | --------------------------- 186 | 187 | procedure Initialize_Ultrasonic (Sensor : out Ultrasonic_Sonar_Sensor) is 188 | pragma SPARK_Mode (Off); 189 | Successful : Boolean; 190 | Max_Attempts : constant := 3; -- arbitrary 191 | use Hardware_Configuration; 192 | begin 193 | Sensor.Configure 194 | (Data_Line => Sonar_Data_Pin, 195 | Clock_Line => Sonar_Clock_Pin, 196 | Clock_Frequency => Sonar_Clock_Frequency, 197 | Success => Successful); 198 | if not Successful then 199 | raise Program_Error; 200 | end if; 201 | 202 | Enabling : for Attempt in 1 .. Max_Attempts loop 203 | Sensor.Enable (Mode => Continuous, IO_Successful => Successful); 204 | exit Enabling when Successful; 205 | if Attempt = Max_Attempts then 206 | raise Program_Error; 207 | end if; 208 | end loop Enabling; 209 | 210 | Resetting : for Attempt in 1 .. Max_Attempts loop 211 | Sensor.Warm_Restart (Successful); 212 | exit Resetting when Successful; 213 | if Attempt = Max_Attempts then 214 | raise Program_Error; 215 | end if; 216 | end loop Resetting; 217 | end Initialize_Ultrasonic; 218 | 219 | ---------------- 220 | -- Initialize -- 221 | ---------------- 222 | 223 | procedure Initialize is 224 | begin 225 | Initialize_Motors (Steering => Steering_Motor, Locomotion => Engine); 226 | Initialize_Ultrasonic (Sensor => Sonar); 227 | end Initialize; 228 | 229 | end Vehicle; 230 | -------------------------------------------------------------------------------- /src/vehicle.ads: -------------------------------------------------------------------------------- 1 | -- This package represents the physical plant, ie the RC car itself. As such, 2 | -- it contains the declarations for the two NXT motors and the NXT ultrasonic 3 | -- "sonar" sensor. The motors are used for propulsion and steering, and the 4 | -- sonar is used for collision avoidance. Other physical artifacts are also 5 | -- provided here, such as the mapping of turning directions to motor rotation 6 | -- direction, the wheel size and gear ratio, and so forth. Finally, the package 7 | -- provides runtime information about the car, such as the current speed (in 8 | -- centimeters/sec). 9 | -- 10 | -- The physical car is based directly on the HiTechnic IR RC Car, with several 11 | -- significant differences. Not least is the fact that no LEGO NXT brick is 12 | -- used, requiring a "cage" to hold the selected MCU card as well as a place to 13 | -- put the third-party electronics card used to interface to the NXT motors and 14 | -- sonar sensor. 15 | -- 16 | -- Instructions for building their original car are here: 17 | -- http://www.hitechnic.com/models 18 | 19 | with System_Configuration; 20 | with NXT.Motors; use NXT.Motors; 21 | with NXT.Ultrasonic_Sensors; use NXT.Ultrasonic_Sensors; 22 | with Remote_Control; use Remote_Control; 23 | 24 | package Vehicle with 25 | SPARK_Mode, 26 | Abstract_State => (Internal_State with Synchronous), 27 | Initializes => (Internal_State) 28 | -- Initializes => (Engine, Steering_Motor, Sonar, Internal_State), 29 | -- Initial_Condition => Configured (Sonar) and then Enabled (Sonar) and then Current_Scan_Mode (Sonar) = Continuous 30 | is 31 | 32 | pragma Elaborate_Body; 33 | 34 | procedure Initialize with 35 | Post => Configured (Sonar) and then Enabled (Sonar) and then Current_Scan_Mode (Sonar) = Continuous; 36 | 37 | Engine : Basic_Motor; -- this is NXT_Shield.Motor2, as physically connected 38 | Steering_Motor : Basic_Motor; -- this is NXT_Shield.Motor1, as physically connected 39 | 40 | Sonar : Ultrasonic_Sonar_Sensor (Hardware_Device_Address => 1); 41 | -- this sensor is always on the third port of the NXT_Shield, dedicated to 42 | -- sensors that require +9V power 43 | 44 | To_The_Right : constant NXT.Motors.Directions := Backward; 45 | To_The_Left : constant NXT.Motors.Directions := Forward; 46 | -- The mapping of steering directions to motor directions. These values 47 | -- reflect the physical construction of the vehicle. 48 | 49 | function To_Steering_Motor_Direction (Power : Float) return NXT.Motors.Directions 50 | with Inline; 51 | -- Map the input power to a motor rotation direction, ie one of the above 52 | -- constants. The choice of To_The_Right versus To_The_Left reflects the 53 | -- steering mechanism's physical construction with the steering motor. 54 | 55 | function To_Propulsion_Motor_Direction (Direction : Remote_Control.Travel_Directions) 56 | return NXT.Motors.Directions 57 | with Inline, 58 | Pre => Direction /= Remote_Control.Neither; 59 | -- Map the input travel direction to a motor rotation direction. The result 60 | -- reflects the drive mechanism's physical construction with the propulsion 61 | -- motor. 62 | 63 | function Speed return Float with Inline, Volatile_Function; 64 | -- in cm/sec 65 | 66 | function Odometer return Float with Inline, Volatile_Function; 67 | -- in centimeters 68 | 69 | Sonar_Offset_From_Front : constant Centimeters := 8; 70 | -- The offset of the ultrasonic sensor from the front of the vehicle as 71 | -- physically built. You need to change this if you move the sonar sensor. 72 | 73 | -- The wheel diameter and gear ratio are used in the package body to 74 | -- calculate the speed, but might be useful elsewhere within the code. 75 | -- Therefore we make their declarations visible. 76 | -- 77 | -- NOTE: if you use different wheels or change the gear ratio you must 78 | -- change these values! Otherwise the speed and distance traveled 79 | -- calculations will be wrong. 80 | 81 | Wheel_Diameter : constant := 5.6; -- centimeters 82 | -- This is the diameter for the Lego NXT 56x26mm wheels. 83 | 84 | Gear_Ratio : constant := 24.0 / 40.0; 85 | -- This is the gear ratio using the 24-tooth gear on the differential 86 | -- as the driven output gear and the 40-tooth spur gear as the driver gear 87 | -- connected to the motor. 88 | -- 89 | -- LEGO Technic Differential Gear Complete Set 24-16 Teeth (part 6573) and 90 | -- Bevel Gears 12-Tooth (part 6589) 91 | 92 | private 93 | 94 | task Engine_Monitor with 95 | Part_Of => Internal_State, 96 | Storage_Size => 1 * 1024, 97 | Priority => System_Configuration.Engine_Monitor_Priority; 98 | 99 | end Vehicle; 100 | --------------------------------------------------------------------------------