Embedded Files
UWB Scanner & Bluetooth Receiver with MAG3110 Magnetometer (ESP32)
UWB Scanner & Bluetooth Receiver with MAG3110 Magnetometer (ESP32)
/*********
Rui Santos
Complete instructions at https://RandomNerdTutorials.com/esp32-ble-server-client/
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files.
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*********/
#include "BLEDevice.h"
#include <Wire.h>
#include <Arduino.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <SparkFun_MAG3110.h>
#define MAG_ADDR 0x0E
#define UWB_INDEX 0
#define TAG
// #define ANCHOR
#define FREQ_850K
// #define FREQ_6800K
#define UWB_TAG_COUNT 32
#define SERIAL_LOG Serial
#define SERIAL_AT mySerial2
HardwareSerial SERIAL_AT(2);
// RESET Pin
#define RESET 32
// ESP32 UART Communication w/ UWB Chip
#define IO_RXD2 18
#define IO_TXD2 19
// MAG3110
#define I2C_SDA_1 25
#define I2C_SCL_1 26
#define I2C_1_Freq 400000
//Instantiate MAG3110
MAG3110 mag = MAG3110();
//BLE Server name (the other ESP32 name running the server sketch)
#define bleServerName_0 "ANCHOR_0"
#define bleServerName_1 "ANCHOR_1"
#define bleServerName_2 "ANCHOR_2"
/* UUID's of the service, characteristic that we want to read*/
// Anchor0 (Server 0) BLE Services
static BLEUUID bmeService_0_0_UUID("91bad492-b950-4226-aa2b-4ede9fa42f59");
static BLEUUID bmeService_0_1_UUID("1109555b-0dca-4eae-8794-36e005c463a2");
// Anchor1 (Server 1) BLE Services
static BLEUUID bmeService_1_0_UUID("cfb729d0-9123-4a59-9b2a-2ca0ad0bd4cd");
static BLEUUID bmeService_1_1_UUID("f2c1d520-e889-42cf-b45f-66a9a9879707");
// Anchor2 (Server 2) BLE Services
static BLEUUID bmeService_2_0_UUID("15240037-9a4c-48b0-bfdb-b4ada6204d70");
static BLEUUID bmeService_2_1_UUID("23d1f9ed-96a2-4dc6-a924-43682544c45f");
// BLE Characteristics
static BLEUUID Angle_0_CharacteristicUUID("cba1d466-344c-4be3-ab3f-189f80dd7518");
static BLEUUID Angle_1_CharacteristicUUID("cba1d466-344c-4be3-ab3f-189f80dd7518");
//Flags stating if should begin connecting and if the connection is up
static boolean doConnect = false;
static boolean connected = false;
//Address of the peripheral device. Address will be found during scanning...
static BLEAddress *pServerAddress_0;
static BLEAddress *pServerAddress_1;
static BLEAddress *pServerAddress_2;
//Characteristicd that we want to read
static BLERemoteCharacteristic* Angle_0_Characteristic;
static BLERemoteCharacteristic* Angle_1_Characteristic;
//Activate notify
const uint8_t notificationOn[] = {0x1, 0x0};
const uint8_t notificationOff[] = {0x0, 0x0};
//Variables to store angle
uint16_t ptr;
int angle_0_0, angle_1_0, angle_2_0, angle_0_1, angle_1_1, angle_2_1;
//Flags to check whether new angle data is available
boolean newAngle_0_0 = false;
boolean newAngle_0_1 = false;
boolean newAngle_1_0 = false;
boolean newAngle_1_1 = false;
boolean newAngle_2_0 = false;
boolean newAngle_2_1 = false;
long int runtime = 0;
int i;
bool deviceConnected = false;
int MAG_X, MAG_Y, MAG_Z;
float Heading_0, Heading_1, Heading_ToT;
String response = "";
String rec_head = "AT+RANGE";
bool print = false;
// Calibration for MAG3110
int Y_OFFSET = 3900;
int Z_OFFSET = 1550;
//Connect to the BLE Server that has the name, Service, and Characteristics
bool connectToAnchor_0(BLEAddress pAddress_0) {
BLEClient* pClient = BLEDevice::createClient();
// Connect to the remote BLE Server.
pClient->connect(pAddress_0);
Serial.println(" - Connected to Anchor0");
// Obtain a reference to the first service we are after in the remote BLE server.
BLERemoteService* pRemoteService_0 = pClient->getService(bmeService_0_0_UUID);
if (pRemoteService_0 == nullptr) {
Serial.print("Failed to find our service UUID: ");
Serial.println(bmeService_0_0_UUID.toString().c_str());
return (false);
}
// Obtain a reference to the first characteristics in the first service of the remote BLE server.
Angle_0_Characteristic = pRemoteService_0->getCharacteristic(Angle_0_CharacteristicUUID);
// add "|| CHARACTERISTIC_ _Characteristic == nullptr" for every extra characteristic
if (Angle_0_Characteristic == nullptr) {
Serial.print("Failed to find our first characteristic UUID");
return false;
}
// Obtain a reference to the second service we are after in the remote BLE server.
BLERemoteService* pRemoteService_1 = pClient->getService(bmeService_0_1_UUID);
if (pRemoteService_1 == nullptr) {
Serial.print("Failed to find our service UUID: ");
Serial.println(bmeService_0_1_UUID.toString().c_str());
return (false);
}
// Obtain a reference to the second characteristics in the second service of the remote BLE server.
Angle_1_Characteristic = pRemoteService_1->getCharacteristic(Angle_1_CharacteristicUUID);
if (Angle_1_Characteristic == nullptr) {
Serial.print("Failed to find our second characteristic UUID");
return false;
}
Serial.println(" - Found our characteristics");
//Assign callback functions for the Characteristics
Angle_0_Characteristic->registerForNotify(Angle_0_0_NotifyCallback);
Angle_1_Characteristic->registerForNotify(Angle_0_1_NotifyCallback);
return true;
}
//Connect to the BLE Server that has the name, Service, and Characteristics
bool connectToAnchor_1(BLEAddress pAddress_1) {
BLEClient* pClient = BLEDevice::createClient();
// Connect to the remote BLE Server.
pClient->connect(pAddress_1);
Serial.println(" - Connected to Anchor1");
// Obtain a reference to the first service we are after in the remote BLE server.
BLERemoteService* pRemoteService_0 = pClient->getService(bmeService_1_0_UUID);
if (pRemoteService_0 == nullptr) {
Serial.print("Failed to find our service UUID: ");
Serial.println(bmeService_1_0_UUID.toString().c_str());
return (false);
}
// Obtain a reference to the first characteristics in the first service of the remote BLE server.
Angle_0_Characteristic = pRemoteService_0->getCharacteristic(Angle_0_CharacteristicUUID);
// add "|| CHARACTERISTIC_ _Characteristic == nullptr" for every extra characteristic
if (Angle_0_Characteristic == nullptr) {
Serial.print("Failed to find our first characteristic UUID");
return false;
}
// Obtain a reference to the second service we are after in the remote BLE server.
BLERemoteService* pRemoteService_1 = pClient->getService(bmeService_1_1_UUID);
if (pRemoteService_1 == nullptr) {
Serial.print("Failed to find our service UUID: ");
Serial.println(bmeService_1_1_UUID.toString().c_str());
return (false);
}
// Obtain a reference to the second characteristics in the second service of the remote BLE server.
Angle_1_Characteristic = pRemoteService_1->getCharacteristic(Angle_1_CharacteristicUUID);
if (Angle_1_Characteristic == nullptr) {
Serial.print("Failed to find our second characteristic UUID");
return false;
}
Serial.println(" - Found our characteristics");
//Assign callback functions for the Characteristics
Angle_0_Characteristic->registerForNotify(Angle_1_0_NotifyCallback);
Angle_1_Characteristic->registerForNotify(Angle_1_1_NotifyCallback);
return true;
}
//Connect to the BLE Server that has the name, Service, and Characteristics
bool connectToAnchor_2(BLEAddress pAddress_2) {
BLEClient* pClient = BLEDevice::createClient();
// Connect to the remote BLE Server.
pClient->connect(pAddress_2);
Serial.println(" - Connected to Anchor2");
// Obtain a reference to the first service we are after in the remote BLE server.
BLERemoteService* pRemoteService_0 = pClient->getService(bmeService_2_0_UUID);
if (pRemoteService_0 == nullptr) {
Serial.print("Failed to find our service UUID: ");
Serial.println(bmeService_2_0_UUID.toString().c_str());
return (false);
}
// Obtain a reference to the first characteristics in the first service of the remote BLE server.
Angle_0_Characteristic = pRemoteService_0->getCharacteristic(Angle_0_CharacteristicUUID);
// add "|| CHARACTERISTIC_ _Characteristic == nullptr" for every extra characteristic
if (Angle_0_Characteristic == nullptr) {
Serial.print("Failed to find our characteristic UUID");
return false;
}
// Obtain a reference to the second service we are after in the remote BLE server.
BLERemoteService* pRemoteService_1 = pClient->getService(bmeService_2_1_UUID);
if (pRemoteService_1 == nullptr) {
Serial.print("Failed to find our service UUID: ");
Serial.println(bmeService_2_1_UUID.toString().c_str());
return (false);
}
// Obtain a reference to the second characteristics in the second service of the remote BLE server.
Angle_1_Characteristic = pRemoteService_1->getCharacteristic(Angle_1_CharacteristicUUID);
if (Angle_1_Characteristic == nullptr) {
Serial.print("Failed to find our characteristic UUID");
return false;
}
Serial.println(" - Found our characteristics");
//Assign callback functions for the Characteristics
Angle_0_Characteristic->registerForNotify(Angle_2_0_NotifyCallback);
Angle_1_Characteristic->registerForNotify(Angle_2_1_NotifyCallback);
return true;
}
//Callback function that gets called, when another device's advertisement has been received
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
void onResult(BLEAdvertisedDevice advertisedDevice) {
if (advertisedDevice.getName() == bleServerName_0) { //Check if the name of the advertiser matches
// advertisedDevice.getScan()->stop(); //Scan can be stopped, we found what we are looking for
pServerAddress_0 = new BLEAddress(advertisedDevice.getAddress()); //Address of advertiser is the one we need
// doConnect = true; //Set indicator, stating that we are ready to connect
Serial.println("Anchor0 found.");
}
if (advertisedDevice.getName() == bleServerName_1) { //Check if the name of the advertiser matches
// advertisedDevice.getScan()->stop(); //Scan can be stopped, we found what we are looking for
pServerAddress_1 = new BLEAddress(advertisedDevice.getAddress()); //Address of advertiser is the one we need
// doConnect = true; //Set indicator, stating that we are ready to connect
Serial.println("Anchor1 found.");
}
if (advertisedDevice.getName() == bleServerName_2) { //Check if the name of the advertiser matches
// advertisedDevice.getScan()->stop(); //Scan can be stopped, we found what we are looking for
pServerAddress_2 = new BLEAddress(advertisedDevice.getAddress()); //Address of advertiser is the one we need
// doConnect = true; //Set indicator, stating that we are ready to connect
Serial.println("Anchor2 found.");
}
if((pServerAddress_0 != NULL) && (pServerAddress_1 != NULL) && (pServerAddress_2 != NULL))
{
advertisedDevice.getScan()->stop();
doConnect = true;
Serial.println("All Anchors Found. Conecting!");
}
}
};
//When the BLE Server sends a new angle reading with the notify property
static void Angle_0_0_NotifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic,
uint8_t* pData, size_t length, bool isNotify) {
//store temperature value
ptr = *pData;
angle_0_0 = (int)ptr;
newAngle_0_0 = true;
}
static void Angle_0_1_NotifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic,
uint8_t* pData, size_t length, bool isNotify) {
//store temperature value
ptr = *pData;
angle_0_1 = (int)ptr;
newAngle_0_1 = true;
}
//When the BLE Server sends a new angle reading with the notify property
static void Angle_1_0_NotifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic,
uint8_t* pData, size_t length, bool isNotify) {
//store temperature value
ptr = *pData;
angle_1_0 = (int)ptr;
newAngle_1_0 = true;
}
static void Angle_1_1_NotifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic,
uint8_t* pData, size_t length, bool isNotify) {
//store temperature value
ptr = *pData;
angle_1_1 = (int)ptr;
newAngle_1_1 = true;
}
//When the BLE Server sends a new angle reading with the notify property
static void Angle_2_0_NotifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic,
uint8_t* pData, size_t length, bool isNotify) {
//store temperature value
ptr = *pData;
angle_2_0 = (int)ptr;
newAngle_2_0 = true;
}
static void Angle_2_1_NotifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic,
uint8_t* pData, size_t length, bool isNotify) {
//store temperature value
ptr = *pData;
angle_2_1 = (int)ptr;
newAngle_2_1 = true;
}
//function that prints the latest sensor readings in the OLED display
void printReadings(){
SERIAL_LOG.print(" **** Heading--> A0:");
SERIAL_LOG.print(angle_0_0 + angle_0_1);
SERIAL_LOG.print(", A1:");
SERIAL_LOG.print(angle_1_0 + angle_1_1);
SERIAL_LOG.print(", A2:");
SERIAL_LOG.print(angle_2_0 + angle_2_1);
}
String sendData(String command, const int timeout, boolean debug) {
String response = "";
// command = command + "\r\n";
SERIAL_LOG.println(command);
SERIAL_AT.println(command); // send the read character to the SERIAL_LOG
long int time = millis();
while ((time + timeout) > millis()) {
while (SERIAL_AT.available()) {
// The esp has data so display its output to the serial window
char c = SERIAL_AT.read(); // read the next character.
response += c;
}
}
if (debug) {
SERIAL_LOG.println(response);
}
return response;
}
String config_cmd() {
String temp = "AT+SETCFG=";
// Set device id
temp = temp + UWB_INDEX;
// Set device role
#ifdef TAG
temp = temp + ",0";
#endif
#ifdef ANCHOR
temp = temp + ",1";
#endif
// Set frequence 850k or 6.8M
#ifdef FREQ_850K
temp = temp + ",0";
#endif
#ifdef FREQ_6800K
temp = temp + ",1";
#endif
// Set range filter
temp = temp + ",1";
return temp;
}
String cap_cmd() {
String temp = "AT+SETCAP=";
// Set Tag capacity
temp = temp + UWB_TAG_COUNT;
// Time of a single time slot
#ifdef FREQ_850K
temp = temp + ",15";
#endif
#ifdef FREQ_6800K
temp = temp + ",10";
#endif
return temp;
}
void setup() {
pinMode(RESET, OUTPUT);
digitalWrite(RESET, HIGH);
//Start serial communication
SERIAL_LOG.begin(115200);
SERIAL_LOG.print(F("Hello! ESP32-S3 AT command V1.0 Test"));
SERIAL_AT.begin(115200, SERIAL_8N1, IO_RXD2, IO_TXD2);
SERIAL_AT.println("AT");
Wire.begin(I2C_SDA_1, I2C_SCL_1,I2C_1_Freq);
mag.initialize(); //Initializes the mag sensor
mag.start(); //Puts the sensor in active mode
delay(1000);
sendData("AT?", 2000, 1);
sendData("AT+RESTORE", 5000, 1);
sendData(config_cmd(), 2000, 1);
sendData(cap_cmd(), 2000, 1);
sendData("AT+SETRPT=1", 2000, 1);
sendData("AT+SAVE", 2000, 1);
sendData("AT+RESTART", 2000, 1);
//Init BLE device
BLEDevice::init("");
// Retrieve a Scanner and set the callback we want to use to be informed when we
// have detected a new device. Specify that we want active scanning and start the
// scan to run for 30 seconds.
BLEScan* pBLEScan = BLEDevice::getScan();
pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
pBLEScan->setActiveScan(true);
pBLEScan->start(30);
}
void loop() {
// If the flag "doConnect" is true then we have scanned for and found the desired
// BLE Server with which we wish to connect. Now we connect to it. Once we are
// connected we set the connected flag to be true.
if (doConnect == true) {
if (connectToAnchor_0(*pServerAddress_0)) {
Serial.println("We are now connected to Anchor0.");
//Activate the Notify property of each Characteristic
Angle_0_Characteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)notificationOn, 2, true);
Angle_1_Characteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)notificationOn, 2, true);
connected = true;
}
else {
Serial.println("We have failed to connect to Anchor0; Restart your device to scan for nearby BLE server again.");
}
if (connectToAnchor_1(*pServerAddress_1)) {
Serial.println("We are now connected to Anchor1.");
//Activate the Notify property of each Characteristic
Angle_0_Characteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)notificationOn, 2, true);
Angle_1_Characteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)notificationOn, 2, true);
connected = true;
}
else {
Serial.println("We have failed to connect to Anchor1; Restart your device to scan for nearby BLE server again.");
}
if (connectToAnchor_2(*pServerAddress_2)) {
Serial.println("We are now connected to Anchor2.");
//Activate the Notify property of each Characteristic
Angle_0_Characteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)notificationOn, 2, true);
Angle_1_Characteristic->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t*)notificationOn, 2, true);
connected = true;
}
else {
Serial.println("We have failed to connect to Anchor2; Restart your device to scan for nearby BLE server again.");
}
doConnect = false;
}
//Only read data when it's ready
if((mag.dataReady()) && (print))
{
//Read the data
mag.readMag(&MAG_X, &MAG_Y, &MAG_Z);
// Angle between the Y-axis and Z-axis of the Magnetometer
MAG_Z = MAG_Z - Z_OFFSET;
MAG_Y = MAG_Y - Y_OFFSET;
if(MAG_Z != 0)
{
Heading_ToT = (atan((float)MAG_Y / MAG_Z) * 180) / 3.14;
}
// Avoid division by 0, set denominator to 1 (makes angle = 90)
else if (MAG_Z == 0)
{
MAG_Z = 1;
Heading_ToT = (atan(((float)MAG_Y / MAG_Z)) * 180.0) / 3.14;
}
if((MAG_Z > 0) && (MAG_Y > 0))
{
Heading_ToT += 0;
}
else if((MAG_Z < 0) && (MAG_Y > 0))
{
Heading_ToT += 0;
Heading_ToT += 180;
}
else if((MAG_Z < 0) && (MAG_Y < 0))
{
Heading_ToT += 0;
Heading_ToT += 180;
Heading_ToT += 0;
}
else if((MAG_Z > 0) && (MAG_Y < 0))
{
Heading_ToT += 0;
Heading_ToT += 180;
Heading_ToT += 0;
Heading_ToT += 180;
}
printReadings();
SERIAL_LOG.print(" Scanner: ");
SERIAL_LOG.print((int)Heading_ToT);
SERIAL_LOG.println(" **** ");
print = false;
}
//if new temperature readings are available, print in the OLED
if (((newAngle_0_0) || (newAngle_0_1)) && ((newAngle_1_0) || (newAngle_1_1)) && ((newAngle_2_0) || (newAngle_2_1))){
newAngle_0_0 = false;
newAngle_0_1 = false;
newAngle_1_0 = false;
newAngle_1_1 = false;
newAngle_2_0 = false;
newAngle_2_1 = false;
// print = true;
}
while (SERIAL_LOG.available() > 0) {
SERIAL_AT.write(SERIAL_LOG.read());
yield();
}
while (SERIAL_AT.available() > 0) {
char c = SERIAL_AT.read();
if (c == '\r')
continue;
else if (c == '\n' || c == '\r')
{
SERIAL_LOG.print(response);
print = true;
response = "";
} else
response += c;
}
}
UWB Beacon & Bluetooth Transmitter with MAG3110 Magnetometer (ESP32)
UWB Beacon & Bluetooth Transmitter with MAG3110 Magnetometer (ESP32)
/*********
Rui Santos
Complete instructions at https://RandomNerdTutorials.com/esp32-ble-server-client/
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files.
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*********/
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <BLEAdvertisedDevice.h>
#include <Wire.h>
#include <arduino.h>
#include <math.h>
#include <SparkFun_MAG3110.h>
#define ANCHOR_0
// #define ANCHOR_1
// #define ANCHOR_2
#ifdef ANCHOR_0
#define bleServerName "ANCHOR_0"
#endif
#ifdef ANCHOR_1
#define bleServerName "ANCHOR_1"
#endif
#ifdef ANCHOR_2
#define bleServerName "ANCHOR_2"
#endif
#define I2C_SDA 25
#define I2C_SCL 26
#define I2C_CLOCK 400000
//Instantiate MAG3110
MAG3110 mag = MAG3110();
bool deviceConnected = false;
int MAG_X, MAG_Y, MAG_Z;
float Heading;
uint8_t Heading_out_1;
uint8_t Heading_out_2;
#ifdef ANCHOR_0
// int Y_OFFSET = 3830;
// int Z_OFFSET = 140;
int Y_OFFSET = 3720;
int Z_OFFSET = 240;
#endif
#ifdef ANCHOR_1
int Y_OFFSET = 3730;
int Z_OFFSET = 1400;
#endif
#ifdef ANCHOR_2
int Y_OFFSET = 2500;
int Z_OFFSET = 2800;
#endif
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/
#ifdef ANCHOR_0
#define SERVICE_1_UUID "91bad492-b950-4226-aa2b-4ede9fa42f59"
#define SERVICE_2_UUID "1109555b-0dca-4eae-8794-36e005c463a2"
#endif
#ifdef ANCHOR_1
#define SERVICE_1_UUID "cfb729d0-9123-4a59-9b2a-2ca0ad0bd4cd"
#define SERVICE_2_UUID "f2c1d520-e889-42cf-b45f-66a9a9879707"
#endif
#ifdef ANCHOR_2
#define SERVICE_1_UUID "15240037-9a4c-48b0-bfdb-b4ada6204d70"
#define SERVICE_2_UUID "23d1f9ed-96a2-4dc6-a924-43682544c45f"
#endif
// Angle Characteristic and Descriptor
BLECharacteristic bmeHeading_1_Characteristics("cba1d466-344c-4be3-ab3f-189f80dd7518", BLECharacteristic::PROPERTY_NOTIFY);
BLEDescriptor bmeHeading_1_Descriptor(BLEUUID((uint16_t)0x2902));
BLECharacteristic bmeHeading_2_Characteristics("cba1d466-344c-4be3-ab3f-189f80dd7518", BLECharacteristic::PROPERTY_NOTIFY);
BLEDescriptor bmeHeading_2_Descriptor(BLEUUID((uint16_t)0x2902));
//Setup callbacks onConnect and onDisconnect
class MyServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer* pServer) {
deviceConnected = true;
};
void onDisconnect(BLEServer* pServer) {
deviceConnected = false;
}
};
void setup() {
// Start serial communication
Serial.begin(115200);
Wire.begin(I2C_SDA, I2C_SCL, I2C_CLOCK);
mag.initialize(); //Initializes the mag sensor
mag.start(); //Puts the sensor in active mode
// Serial.setDebugOutput(true);
// Create the BLE Device
BLEDevice::init(bleServerName);
// Create the BLE Server
BLEServer *pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
// Create the BLE Services
BLEService *bmeService_1 = pServer->createService(SERVICE_1_UUID);
BLEService *bmeService_2 = pServer->createService(SERVICE_2_UUID);
// Create BLE Characteristics and Create a BLE Descriptor for each service
bmeService_1->addCharacteristic(&bmeHeading_1_Characteristics);
bmeHeading_1_Descriptor.setValue("BME MAG3110 Heading - first byte");
bmeHeading_1_Characteristics.addDescriptor(&bmeHeading_1_Descriptor);
bmeService_2->addCharacteristic(&bmeHeading_2_Characteristics);
bmeHeading_2_Descriptor.setValue("BME MAG3110 Heading - second byte");
bmeHeading_2_Characteristics.addDescriptor(&bmeHeading_2_Descriptor);
// Start the service
bmeService_1->start();
bmeService_2->start();
// Start advertising
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_1_UUID);
pAdvertising->addServiceUUID(SERVICE_2_UUID);
pServer->getAdvertising()->start();
Serial.println("Waiting a client connection to notify...");
}
void loop() {
if (deviceConnected) {
//Only read data when it's ready
if(mag.dataReady())
{
//Read the data
mag.readMag(&MAG_X, &MAG_Y, &MAG_Z);
// Angle between the Y-axis and Z-axis of the Magnetometer
if(MAG_Z > 5000)
{
MAG_Z = (unsigned int)MAG_Z - 65580;
}
else
{
MAG_Z = (unsigned int)MAG_Z - Z_OFFSET;
}
MAG_Y = MAG_Y - Y_OFFSET;
if(MAG_Z != 0)
{
Heading = (atan((float)MAG_Y / MAG_Z) * 180) / 3.14;
}
// Avoid division by 0, set denominator to 1 (makes angle = 90)
else if (MAG_Z == 0)
{
MAG_Z = 1;
Heading = (atan(((float)MAG_Y / MAG_Z)) * 180.0) / 3.14;
}
if((MAG_Z > 0) && (MAG_Y > 0))
{
Heading += 0;
}
else if((MAG_Z < 0) && (MAG_Y > 0))
{
Heading += 0;
Heading += 180;
}
else if((MAG_Z < 0) && (MAG_Y < 0))
{
Heading += 0;
Heading += 180;
Heading += 0;
}
else if((MAG_Z > 0) && (MAG_Y < 0))
{
Heading += 0;
Heading += 180;
Heading += 0;
Heading += 180;
}
if(Heading <= 180)
{
Heading_out_1 = (uint8_t)Heading;
Heading_out_2 = (uint8_t)0;
}
else if(Heading > 180)
{
Heading_out_1 = (uint8_t)180;
Heading_out_2 = (uint8_t)(Heading - 180.0);
}
Serial.print("Heading - first byte: ");
Serial.println(Heading_out_1);
Serial.print("Heading - second byte: ");
Serial.println(Heading_out_2);
}
//Set angle Characteristic value and notify connected client
int m = Heading_out_1;
bmeHeading_1_Characteristics.setValue(m);
bmeHeading_1_Characteristics.notify();
int d = Heading_out_2;
bmeHeading_2_Characteristics.setValue(d);
bmeHeading_2_Characteristics.notify();
delay(50);
}
}
Main Navigational System Code (KL25Z)
Main Navigational System Code (KL25Z)
/* ###################################################################
** Filename : main.c
** Project : Groomba_Automation_Control
** Processor : MKL25Z128VLK4
** Version : Driver 01.01
** Compiler : GNU C Compiler
** Date/Time : 2024-02-06, 08:22, # CodeGen: 0
** Abstract :
** Main module.
** This module contains user's application code.
** Settings :
** Contents :
** No public methods
**
** ###################################################################*/
/*!
** @file main.c
** @version 01.01
** @brief
** Main module.
** This module contains user's application code.
*/
/*!
** @addtogroup main_module main module documentation
** @{
*/
/* MODULE main */
/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "AS1.h"
#include "ASerialLdd1.h"
#include "Bit1.h"
#include "BitIoLdd1.h"
#include "Bit2.h"
#include "BitIoLdd2.h"
#include "Bit3.h"
#include "BitIoLdd3.h"
#include "TI1.h"
#include "TimerIntLdd1.h"
#include "TU1.h"
#include "CI2C1.h"
#include "IntI2cLdd1.h"
/* Including shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"
/* User includes (#include below this line is not maintained by Processor Expert) */
#include "delays.h"
#include "RTLS.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "Shield_OLED_display.h"
// MPU-9250 I2C Address (Accelerometer & Gyroscope)
#define SLAVE 0x0E
// Max Y-axis offset
#define Y_OFFSET_CONST 4080
// Max Z-axis offset
#define Z_OFFSET_CONST 1900
#define Q1 0
#define Q2 180
#define Q3 0
#define Q4 180
byte err, flag, err2, flag2, err3;
byte addr2[18], addr3[2], Values[4];
byte addr = (byte)0x00;
word sent, sent2;
char target_msg[] = "range:(";
char target_received[7];
float dist_A0, dist_A1, dist_A2;
int angle_A0, angle_A1, angle_A2, angle_T0;
AS1_TComData ch[200];
word Received;
char msg[20];
signed int dummyx, dummyy, dummyz;
float angle_yz, angle_travel, angle_current, angle_next;
int y_off, z_off;
int start_of_data, start_of_data_2, start_of_data_3, start_of_data_4, start_of_data_5, start_of_data_6, start_of_data_7, i, j;
float map[36][3];
float travel[36];
float SGx, SGy;
float distance_beacon;
float closest_target, current_target, next_target;
int ind;
int GroombaOnTarget;
/*lint -save -e970 Disable MISRA rule (6.3) checking. */
int main(void)
/*lint -restore Enable MISRA rule (6.3) checking. */{
/* Write your local variable definition here */
/*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
PE_low_level_init();
/*** End of Processor Expert internal initialization. ***/
/* Write your code here */
/* For example: for(;;) { } */
// OLED INIT
OLED_initializeDisplay(); // also initializes I2C0
OLED_clearDisplay();
distance_beacon = 5.0;
GroombaOnTarget = 0;
for(i = 0; i < 6; i++)
{
for(j = 0; j < 6; j++)
{
float angle = 0.0;
if(i%2 == 0)
{
angle = 90.0;
}
else
{
angle = 270.0;
}
map[j + (i*6)][0] = (float)i;
map[j + (i*6)][1] = (float)(0.0 + (j*2));
map[j + (i*6)][2] = angle;
}
}
// MAG SETUP
/* This sequence activates the automatic magnetic sensor reset before each data acquisition
* by setting the AUTO_MRST_EN bit high in the CTRL_REG2 register
* additionaly, the RAW bit is set low, enabling the implementation of user offset on the output
*/
// Register address (CTRL_REG2)
// addr3[0] = (byte) 0x11;
// // Register mask (1000 0000)
// addr3[1] = (byte) 0x80;
// //Sets the 7-Bit Slave Address for I2C communication
// CI2C1_SelectSlave(SLAVE);
// // write-data transmission complete flag is reset
// flag = 0;
// // data is sent to I2C Bus
// err = CI2C1_SendBlock(&addr3, sizeof(addr3), &sent);
// // Wait until transmission is done (write-data flag is set)
// while (!flag);
/* This sequence puts the Magnetometer into ACTIVE mode
* by setting the AC(Operating Mode) bit high in the CTRL_REG1 register
* Magnetometer must be in ACTIVE mode to measure data, otherwise all data is 0's
*/
// Register address (CTRL_REG1)
// addr3[0] = (byte) 0x10;
// // Register mask (0000 0001)
// addr3[1] = (byte) 0x01;
// //Sets the 7-Bit Slave Address for I2C communication
// CI2C1_SelectSlave(SLAVE);
// // write-data transmission complete flag is reset
// flag = 0;
// // data is sent to I2C Bus
// err = CI2C1_SendBlock(&addr3, sizeof(addr3), &sent);
// // Wait until transmission is done (write-data flag is set)
// while (!flag);
// Autonomous System Loop
for(;;)
{
// //I2C COMMMS
//Sets the 7-Bit Slave Address for I2C communication
// CI2C1_SelectSlave(SLAVE);
// // write-data transmission complete flag is reset
// flag = 0;
// // data is sent to I2C Bus
// err = CI2C1_SendBlock(&addr, sizeof(addr), &sent);
// // Wait until transmission is done (write-data flag is set)
// while (!flag);
//
// //Sets the 7-Bit Slave Address for I2C communication
// CI2C1_SelectSlave(SLAVE);
// // read-data transmission complete flag is reset
// flag2 = 0;
// // data is recovered from I2C Bus
// err2 = CI2C1_RecvBlock(&addr2, sizeof(addr2), &sent2);
// // Wait until transmission is done (read-data flag is set)
// while (!flag2);
//
// y_off = Y_OFFSET_CONST;
// z_off = Z_OFFSET_CONST;
// // MAG DATA
// // Concatenate Y-axis MSB and LSB into one variable and apply offset
// dummyy = (addr2[3] * 256) + addr2[4] - y_off;
// Print 2-Bit Y-axis Magnetometer Data
// OLED_setLocation(0, 3);
// sprintf(msg, "Ydata: % 6i ", dummyy);
// OLED_writeString(msg);
// // Concatenate Z-axis MSB and LSB into one variable and apply offset
// dummyz = (addr2[5] * 256) + addr2[6] - z_off;
// Print 2-Bit Z-axis Magnetometer Data
// OLED_setLocation(0, 4);
// sprintf(msg, "Zdata: % 6i ", dummyz);
// OLED_writeString(msg);
// Y-Z ANGLE
// Angle between the Y-axis and Z-axis of the Magnetometer
// if (dummyz != 0) {
// angle_yz = (atan(((float) dummyy / dummyz)) * 180.0) / 3.14;
// }
// // Avoid division by 0, set denominator to 1 (makes angle = 90)
// else if (dummyz == 0) {
// dummyz = 1;
// angle_yz = (atan(((float) dummyy / dummyz)) * 180.0) / 3.14;
// }
//
// if((dummyz > 0) && (dummyy > 0))
// {
// angle_yz += Q1;
// }
// else if((dummyz < 0) && (dummyy > 0))
// {
// angle_yz += Q1;
// angle_yz += Q2;
// }
// else if((dummyz < 0) && (dummyy < 0))
// {
// angle_yz += Q1;
// angle_yz += Q2;
// angle_yz += Q3;
// }
// else if((dummyz > 0) && (dummyy < 0))
// {
// angle_yz += Q1;
// angle_yz += Q2;
// angle_yz += Q3;
// angle_yz += Q4;
// }
// Print Angle
// OLED_setLocation(0, 6);
// sprintf(msg, "Heading: % 4.0f", angle_yz);
// OLED_writeString(msg);
//
// Receive a Block of Data From The Tag
err3 = AS1_RecvBlock((byte*) &ch, sizeof(ch), &Received);
if((int)ch[0] != 0)
{
// Search the array of data for "range:("
for(i = 0; i < (sizeof(ch) - 6); i++)
{
// Store 4 elements of the array at a time
// check to see if sample of array matches "range:("
if((int)ch[i] == 114)
{
target_received[0] = ch[i]; // 'r'
if((int)ch[i + 1] == 97)
{
target_received[1] = ch[i + 1]; // 'a'
}
if((int)ch[i + 2] == 110)
{
target_received[2] = ch[i + 2]; // 'n'
}
if((int)ch[i + 3] == 103)
{
target_received[3] = ch[i + 3]; // 'g'
}
if((int)ch[i + 4] == 101)
{
target_received[4] = ch[i + 4]; // 'e'
}
if((int)ch[i + 5] == 58)
{
target_received[5] = ch[i + 5]; // ':'
}
if((int)ch[i + 6] == 40)
{
target_received[6] = ch[i + 6]; // '('
}
// if the message "range:(" is found, store the index just after the message, where the data starts
if(strncmp(target_received, target_msg, sizeof(target_received)) == 0)
{
// Store index were range values start
start_of_data = i + 7;
}
}
// DISTANCE ANCHOR 0
// Converts char numerals to decimals, handle different sized data (Ex. 1, or 12, or 134), if the data @ start_of_data is a numeral
if(((int)ch[start_of_data] > 47) && ((int)ch[start_of_data] < 58))
{
// If data is 3 digits long (if the fourth element is a comma)
if((int)ch[start_of_data + 3] == 44)
{
// Converts first, second, and third digits into decimals, discard fourth element
dist_A0 = ((int)ch[start_of_data] - 48)*100 + ((int)ch[start_of_data + 1] - 48)*10 + ((int)ch[start_of_data + 2] - 48);
dist_A0 /= 124.3;
dist_A0 *= 3.28;
start_of_data_2 = start_of_data + 4;
}
// If data is 2 digits long (if third element is a comma & fourth element is not a comma)
else if(((int)ch[start_of_data + 2] == 44) && ((int)ch[start_of_data + 3] != 44))
{
// Converts first, and second digits into decimals, discard third and fourth elements
dist_A0 = ((int)ch[start_of_data] - 48)*10 + ((int)ch[start_of_data + 1] - 48);
dist_A0 /= 124.3;
dist_A0 *= 3.28;
start_of_data_2 = start_of_data + 3;
}
// If data is 1 digit long (if second element is a comma & third element is not a comma
else if(((int)ch[start_of_data + 1] == 44) && ((int)ch[start_of_data + 2] != 44))
{
// Converts first digit into a decimal, discard second, third, and fourth elements
dist_A0 = ((int)ch[start_of_data] - 48);
dist_A0 /= 124.3;
dist_A0 *= 3.28;
start_of_data_2 = start_of_data + 2;
}
}
// DISTANCE ANCHOR 1
// Converts char numerals to decimals, handle different sized data (Ex. 1, or 12, or 134), if the data @ start_of_data is a numeral
if(((int)ch[start_of_data_2] > 47) && ((int)ch[start_of_data_2] < 58))
{
// If data is 3 digits long (if the fourth element is a comma)
if((int)ch[start_of_data_2 + 3] == 44)
{
// Converts first, second, and third digits into decimals, discard fourth element
dist_A1 = ((int)ch[start_of_data_2] - 48)*100 + ((int)ch[start_of_data_2 + 1] - 48)*10 + ((int)ch[start_of_data_2 + 2] - 48);
dist_A1 /= 124.3;
dist_A1 *= 3.28;
start_of_data_3 = start_of_data_2 + 4;
}
// If data is 2 digits long (if third element is a comma & fourth element is not a comma)
else if(((int)ch[start_of_data_2 + 2] == 44) && ((int)ch[start_of_data_2 + 3] != 44))
{
// Converts first, and second digits into decimals, discard third and fourth elements
dist_A1 = ((int)ch[start_of_data_2] - 48)*10 + ((int)ch[start_of_data_2 + 1] - 48);
dist_A1 /= 124.3;
dist_A1 *= 3.28;
start_of_data_3 = start_of_data_2 + 3;
}
// If data is 1 digit long (if second element is a comma & third element is not a comma
else if(((int)ch[start_of_data_2 + 1] == 44) && ((int)ch[start_of_data_2 + 2] != 44))
{
// Converts first digit into a decimal, discard second, third, and fourth elements
dist_A1 = ((int)ch[start_of_data_2] - 48);
dist_A1 /= 124.3;
dist_A1 *= 3.28;
start_of_data_3 = start_of_data_2 + 2;
}
}
// DISTANCE ANCHOR 2
// Converts char numerals to decimals, handle different sized data (Ex. 1, or 12, or 134), if the data @ start_of_data is a numeral
if(((int)ch[start_of_data_3] > 47) && ((int)ch[start_of_data_3] < 58))
{
// If data is 3 digits long (if the fourth element is a comma)
if((int)ch[start_of_data_3 + 3] == 44)
{
// Converts first, second, and third digits into decimals, discard fourth element
dist_A2 = ((int)ch[start_of_data_3] - 48)*100 + ((int)ch[start_of_data_3 + 1] - 48)*10 + ((int)ch[start_of_data_3 + 2] - 48);
dist_A2 /= 124.3;
dist_A2 *= 3.28;
start_of_data_4 = start_of_data_3 + 87;
}
// If data is 2 digits long (if third element is a comma & fourth element is not a comma)
else if(((int)ch[start_of_data_3 + 2] == 44) && ((int)ch[start_of_data_3 + 3] != 44))
{
// Converts first, and second digits into decimals, discard third and fourth elements
dist_A2 = ((int)ch[start_of_data_3] - 48)*10 + ((int)ch[start_of_data_3 + 1] - 48);
dist_A2 /= 124.3;
dist_A2 *= 3.28;
start_of_data_4 = start_of_data_3 + 86;
}
// If data is 1 digit long (if second element is a comma & third element is not a comma
else if(((int)ch[start_of_data_3 + 1] == 44) && ((int)ch[start_of_data_3 + 2] != 44))
{
// Converts first digit into a decimal, discard second, third, and fourth elements
dist_A2 = ((int)ch[start_of_data_3] - 48);
dist_A2 /= 124.3;
dist_A2 *= 3.28;
start_of_data_4 = start_of_data_3 + 85;
}
}
// ANGLE ANCHOR 0
// Converts char numerals to decimals, handle different sized data (Ex. 1, or 12, or 134), if the data @ start_of_data is a numeral
if(((int)ch[start_of_data_4] > 47) && ((int)ch[start_of_data_4] < 58))
{
// If data is 3 digits long (if the fourth element is a comma)
if((int)ch[start_of_data_4 + 3] == 44)
{
// Converts first, second, and third digits into decimals, discard fourth element
angle_A0 = ((int)ch[start_of_data_4] - 48)*100 + ((int)ch[start_of_data_4 + 1] - 48)*10 + ((int)ch[start_of_data_4 + 2] - 48);
start_of_data_5 = start_of_data_4 + 8;
}
// If data is 2 digits long (if third element is a comma & fourth element is not a comma)
else if(((int)ch[start_of_data_4 + 2] == 44) && ((int)ch[start_of_data_4 + 3] != 44))
{
// Converts first, and second digits into decimals, discard third and fourth elements
angle_A0 = ((int)ch[start_of_data_4] - 48)*10 + ((int)ch[start_of_data_4 + 1] - 48);
start_of_data_5 = start_of_data_4 + 7;
}
// If data is 1 digit long (if second element is a comma & third element is not a comma
else if(((int)ch[start_of_data_4 + 1] == 44) && ((int)ch[start_of_data_4 + 2] != 44))
{
// Converts first digit into a decimal, discard second, third, and fourth elements
angle_A0 = ((int)ch[start_of_data_4] - 48);
start_of_data_5 = start_of_data_4 + 6;
}
}
// ANGLE ANCHOR 1
// Converts char numerals to decimals, handle different sized data (Ex. 1, or 12, or 134), if the data @ start_of_data is a numeral
if(((int)ch[start_of_data_5] > 47) && ((int)ch[start_of_data_5] < 58))
{
// If data is 3 digits long (if the fourth element is a comma)
if((int)ch[start_of_data_5 + 3] == 44)
{
// Converts first, second, and third digits into decimals, discard fourth element
angle_A1 = ((int)ch[start_of_data_5] - 48)*100 + ((int)ch[start_of_data_5 + 1] - 48)*10 + ((int)ch[start_of_data_5 + 2] - 48);
start_of_data_6 = start_of_data_5 + 8;
}
// If data is 2 digits long (if third element is a comma & fourth element is not a comma)
else if(((int)ch[start_of_data_5 + 2] == 44) && ((int)ch[start_of_data_5 + 3] != 44))
{
// Converts first, and second digits into decimals, discard third and fourth elements
angle_A1 = ((int)ch[start_of_data_5] - 48)*10 + ((int)ch[start_of_data_5 + 1] - 48);
start_of_data_6 = start_of_data_5 + 7;
}
// If data is 1 digit long (if second element is a comma & third element is not a comma
else if(((int)ch[start_of_data_5 + 1] == 44) && ((int)ch[start_of_data_5 + 2] != 44))
{
// Converts first digit into a decimal, discard second, third, and fourth elements
angle_A1 = ((int)ch[start_of_data_5] - 48);
start_of_data_6 = start_of_data_5 + 6;
}
}
// ANGLE ANCHOR 2
// Converts char numerals to decimals, handle different sized data (Ex. 1, or 12, or 134), if the data @ start_of_data is a numeral
if(((int)ch[start_of_data_6] > 47) && ((int)ch[start_of_data_6] < 58))
{
// If data is 3 digits long (if the fourth element is a comma)
if((int)ch[start_of_data_6 + 3] == 32)
{
// Converts first, second, and third digits into decimals, discard fourth element
angle_A2 = ((int)ch[start_of_data_6] - 48)*100 + ((int)ch[start_of_data_6 + 1] - 48)*10 + ((int)ch[start_of_data_6 + 2] - 48);
start_of_data_7 = start_of_data_6 + 13;
}
// If data is 2 digits long (if third element is a comma & fourth element is not a comma)
else if(((int)ch[start_of_data_6 + 2] == 32) && ((int)ch[start_of_data_6 + 3] != 32))
{
// Converts first, and second digits into decimals, discard third and fourth elements
angle_A2 = ((int)ch[start_of_data_6] - 48)*10 + ((int)ch[start_of_data_6 + 1] - 48);
start_of_data_7 = start_of_data_6 + 12;
}
// If data is 1 digit long (if second element is a comma & third element is not a comma
else if(((int)ch[start_of_data_6 + 1] == 32) && ((int)ch[start_of_data_6 + 2] != 32))
{
// Converts first digit into a decimal, discard second, third, and fourth elements
angle_A2 = ((int)ch[start_of_data_6] - 48);
start_of_data_7 = start_of_data_6 + 11;
}
}
// ANGLE TAG
// Converts char numerals to decimals, handle different sized data (Ex. 1, or 12, or 134), if the data @ start_of_data is a numeral
if(((int)ch[start_of_data_7] > 47) && ((int)ch[start_of_data_7] < 58))
{
// If data is 3 digits long (if the fourth element is a comma)
if((int)ch[start_of_data_7 + 3] == 32)
{
// Converts first, second, and third digits into decimals, discard fourth element
angle_T0 = ((int)ch[start_of_data_7] - 48)*100 + ((int)ch[start_of_data_7 + 1] - 48)*10 + ((int)ch[start_of_data_7 + 2] - 48);
}
// If data is 2 digits long (if third element is a comma & fourth element is not a comma)
else if(((int)ch[start_of_data_7 + 2] == 32) && ((int)ch[start_of_data_7 + 3] != 32))
{
// Converts first, and second digits into decimals, discard third and fourth elements
angle_T0 = ((int)ch[start_of_data_7] - 48)*10 + ((int)ch[start_of_data_7 + 1] - 48);
}
// If data is 1 digit long (if second element is a comma & third element is not a comma
else if(((int)ch[start_of_data_7 + 1] == 32) && ((int)ch[start_of_data_7 + 2] != 32))
{
// Converts first digit into a decimal, discard second, third, and fourth elements
angle_T0 = ((int)ch[start_of_data_7] - 48);
}
}
}
}
// OLED_setLocation(0,0);
// sprintf(msg, "%03.2fft", dist_A0);
// OLED_writeString(msg);
//// OLED_setLocation(0,1);
// sprintf(msg, ", %03.2fft", dist_A1);
// OLED_writeString(msg);
// OLED_setLocation(0,1);
// sprintf(msg, "%03.2fft", dist_A2);
// OLED_writeString(msg);
// OLED_setLocation(0,2);
// sprintf(msg, "%03i", angle_A0);
// OLED_writeString(msg);
// sprintf(msg, ", %03i", angle_A1);
// OLED_writeString(msg);
// OLED_setLocation(0,3);
// sprintf(msg, "%03i", angle_A2);
// OLED_writeString(msg);
// Generate real-time (x,y) coordinates for Groomba
SGx = TriangleHeight(dist_A0, dist_A1);
SGy = TriangleHeight(dist_A1, dist_A2);
OLED_setLocation(0,5);
sprintf(msg, "(%02.2fft,", SGx);
OLED_writeString(msg);
sprintf(msg, " %02.2fft)", SGy);
OLED_writeString(msg);
// // If Groomba is off target
// if(GroombaOnTarget == 0)
// {
// // Calculate distance from current position to every target position
// for(i = 0; i < 36; i++)
// {
// travel[i] = dist_h(map[i][0],SGx,map[i][1],SGy);
// }
//
// // Find closest target point
// closest_target = travel[0];
// for(i = 0; i < 36; i++)
// {
// if(closest_target > travel[i])
// {
// ind = i;
// closest_target = travel[i];
//
// // Find angle off from target angle of closest target point
// if(angle_yz > map[i][2])
// {
// angle_travel = angle_yz - map[i][2];
// }
// else if(map[i][2] > angle_yz)
// {
// angle_travel = map[i][2] - angle_yz;
// }
// }
// }
//
// // Check if Groomba is on target, set mapping flag accordingly
// if(closest_target <= 0.4)
// {
// GroombaOnTarget = 1;
// }
// else
// {
// GroombaOnTarget = 0;
// }
// }
// Groomba follows path if on target
// if(GroombaOnTarget == 1)
// {
// float dif = 0.0;
//
// // distance to current target
// current_target = dist_h(map[ind][0],SGx,map[ind][1],SGy);
//
// // angle off from target angle of current target point
// if(angle_yz > map[ind][2])
// {
// angle_current = angle_yz - map[ind][2];
// }
// else if(map[ind][2] > angle_yz)
// {
// angle_current = map[ind][2] - angle_yz;
// }
//
// // distance to next target
// next_target = dist_h(map[ind + 1][0],SGx,map[ind + 1][1],SGy);
//
// // angle off from target angle of next target point
// if(angle_yz > map[ind + 1][2])
// {
// angle_next = angle_yz - map[ind + 1][2];
// }
// else if(map[ind + 1][2] > angle_yz)
// {
// angle_next = map[ind + 1][2] - angle_yz;
// }
//
// // If angle difference is greater than acceptable deviation, correct said angle
// if(angle_current > 5)
// {
// angle_travel = angle_current;
// }
// else if(angle_next > 10)
// {
// angle_travel = angle_next;
// }
//
// // distance to travel
// dif = next_target - current_target;
//
// // Check if Groomba is still on target (distance)
// if(dif > 2.4)
// {
// GroombaOnTarget = 0;
// }
// else
// {
// GroombaOnTarget = 1;
// }
//
// // Increment target counter if close enough to next target
// if (dif < 0.4)
// {
// if(ind < 36)
// {
// ind++;
// }
// else
// {
// ind = ind;
// }
// }
// }
// END OF LOOP
// Loop delay
delay_ms(20);
}
/*** Don't write any code pass this line, or it will be deleted during code generation. ***/
/*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/
#ifdef PEX_RTOS_START
PEX_RTOS_START(); /* Startup of the selected RTOS. Macro is defined by the RTOS component. */
#endif
/*** End of RTOS startup code. ***/
/*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
for(;;){}
/*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/
/* END main */
/*!
** @}
*/
/*
** ###################################################################
**
** This file was created by Processor Expert 10.3 [05.09]
** for the Freescale Kinetis series of microcontrollers.
**
** ###################################################################
*/
Processor Expert Settings For KL25Z Code Above:
RTLS Library Header File for Navigational System Code (KL25Z)
RTLS Library Header File for Navigational System Code (KL25Z)
/*
* RTLS.h
*
* Created on: Mar 8, 2024
* Author: carlo
*/
#ifndef RTLS_H_
#define RTLS_H_
#include <math.h>
#include <stdlib.h>
double DistanceFromRSSI(int r);
double TriangleHeight(double d1, double d2);
double AngleofOrientation(double a, double h);
double RSSI_Avg(int*, int );
double Distance_Avg(double*, int );
int charHEXtoIntDec(char r_10, char r_1);
void CoordComp(float c);
float dist_h(float, float, float, float);
#endif /* RTLS_H_ */
RTLS Library Source File for Navigational System Code (KL25Z)
RTLS Library Source File for Navigational System Code (KL25Z)
/*
* RTLS.c
*
* Created on: Mar 9, 2024
* Author: carlo
*/
#include "RTLS.h"
extern float distance_beacon;
// Generates Distance in feet from an RSSI value (Specific for BT840F BLE SoC PHY: 1MB/S TxPower: 4dB)
double DistanceFromRSSI(int r)
{
// Define Lorentzian Equation Parameters
double ALorentzian = 3.258e4;
double WLorentzian = -7.698e1;
double BLorentzian = 1.569e1;
double CLorentzian = -5.392;
// Meter to Feet Conversion
double meterTofeet = 3.280839895;
// Internal Variables
double p1, p2, p3;
double W, B, R;
double d;
// Lorentzian Best Fit Distance vs. RSSI Equation
W = WLorentzian*WLorentzian;
B = BLorentzian*BLorentzian;
R = r*r;
p1 = W-R;
p1 = p1*p1;
p2 = B*R;
p2 = p2*4;
p3 = sqrt(p1 + p2);
d = (ALorentzian/p3) + CLorentzian;
// Convert Distance in Meters to Feet
d = d * meterTofeet;
// Returns Distance in Feet
return(d);
}
double TriangleHeight(double d1, double d2)
{
// Internal Variables
double h, s, a, b, c, A;
// Use Horan's Formula to Calculate Area of Triangle With 3 Different Side Lenghts
s = (d1 + d2 + distance_beacon)/2;
if(s < d1)
{
a = d1-s;
}
else if(s > d1)
{
a = s-d1;
}
if(s < d2)
{
b = d2-s;
}
else if(s > d2)
{
b = s-d2;
}
if(s < distance_beacon)
{
c = distance_beacon-s;
}
else if(s > distance_beacon)
{
c = s-distance_beacon;
}
A = sqrt(s*a*b*c);
// Use Area to Calculate High (x or y Coordinate of Snow Groomer)
h = (A/distance_beacon)*2;
return(h);
}
// Calculates Angle Opposing the Height of The Triangle w/ Law of Sine
// Use this Angle to Calculate Angle of Incoming BLE Signal
// @ 90 deg (SG Facing Straight Forward at 90 deg)
double AngleofOrientation(double a, double h)
{
double ang_a = 90.0;
double ang_h = 0.0;
double ang_b = 0.0;
double num = 0.0;
// Re-arrange Law of Sine Relationship to Solve for arcsine(opposing_angle)
num = (double)(h/a);
ang_h = asin(num);
// Convert The Angle From Radians to Degrees
ang_h = ang_h / 3.14;
ang_h = ang_h * 180;
// Use Opposing Angle to Calculate BLE Signal Angle
ang_b = 180 - ang_a - ang_h;
/* Return BLE Signal Angle Calculated
* This value will be Compared to The Actual BLE Signal Angle Measured
* and Used to Determine The Angle of Rotation of The Snow Groomer
*/
return(ang_b);
}
// Adds All RSSI Readings and Averages Them to Reduce Error From RSSI Fluctuation
double RSSI_Avg(int *num, int index)
{
double sum = 0;
int i = 0;
// Add All RSSI Values
for(i = 0; i < index; i++)
{
sum = sum + *num;
num++;
}
// Divide by Total Number of Readings
sum = sum/index;
// Return Average
return(sum);
}
// Adds All Distance Readings and Averages Them to Reduce Error From RSSI Fluctuation
double Distance_Avg(double *num, int index)
{
double sum = 0;
int i = 0;
// Add All Distance Values
for(i = 0; i < index; i++)
{
sum = sum + *num;
num++;
}
// Divide by Total Number of Readings
sum /= index;
// Return Average
return(sum);
}
// Converts the 2-character RSSI value (HEX) received from UART into a decimal integer value
int charHEXtoIntDec(char r_1, char r_10)
{
int r;
// Reset RSSI value before storing new value
r = 0;
// Convert the first place of the Hexadecimal value to Decimal
switch(r_1)
{
case('0'):
{
r += 0;
break;
}
case('1'):
{
r += 1;
break;
}
case('2'):
{
r += 2;
break;
}
case('3'):
{
r += 3;
break;
}
case('4'):
{
r += 4;
break;
}
case('5'):
{
r += 5;
break;
}
case('6'):
{
r += 6;
break;
}
case('7'):
{
r += 7;
break;
}
case('8'):
{
r += 8;
break;
}
case('9'):
{
r += 9;
break;
}
case('a'):
{
r += 10;
break;
}
case('b'):
{
r += 11;
break;
}
case('c'):
{
r += 12;
break;
}
case('d'):
{
r += 13;
break;
}
case('e'):
{
r += 14;
break;
}
case('f'):
{
r += 15;
break;
}
default:
{
break;
}
}
// Convert the tenth place of the Hexadecimal value to Decimal
switch(r_10)
{
case('0'):
{
r += (0*16);
break;
}
case('1'):
{
r += (1*16);
break;
}
case('2'):
{
r += (2*16);
break;
}
case('3'):
{
r += (3*16);
break;
}
case('4'):
{
r += (4*16);
break;
}
case('5'):
{
r += (5*16);
break;
}
case('6'):
{
r += (6*16);
break;
}
case('7'):
{
r += (7*16);
break;
}
case('8'):
{
r += (8*16);
break;
}
case('9'):
{
r += (9*16);
break;
}
case('a'):
{
r += (10*16);
break;
}
case('b'):
{
r += (11*16);
break;
}
case('c'):
{
r += (12*16);
break;
}
case('d'):
{
r += (13*16);
break;
}
case('e'):
{
r += (14*16);
break;
}
case('f'):
{
r += (15*16);
break;
}
default:
{
break;
}
}
// Measured Offset in RSSI (Determined With Experimental Testing)
// r = r - 17;
// Return RSSI value (in dB) (positive)
return(r);
}
// Calculates magnitude of diagonal line created from the pair of points (x1, y1) and (x2, y2)
float dist_h(float t_x, float sg_x, float t_y, float sg_y)
{
float n1, n2, n3, hp;
if(t_x > sg_x)
{
n1 = t_x - sg_x;
n1 *= n1;
}
else if(sg_x > t_x)
{
n1 = sg_x - t_x;
n1 *= n1;
}
if(t_y > sg_y)
{
n2 = t_y - sg_y;
n2 *= n2;
}
else if(sg_y > t_y)
{
n2 = sg_y - t_y;
n2 *= n2;
}
n3 = n1 + n2;
hp = sqrtf(n3);
return(hp);
}
Events (Interrupts) Source File for Navigational System Code (KL25Z)
Events (Interrupts) Source File for Navigational System Code (KL25Z)
/* ###################################################################
** Filename : Events.c
** Project : Groomba_Automation_Control
** Processor : MKL25Z128VLK4
** Component : Events
** Version : Driver 01.00
** Compiler : GNU C Compiler
** Date/Time : 2024-02-06, 08:22, # CodeGen: 0
** Abstract :
** This is user's event module.
** Put your event handler code here.
** Settings :
** Contents :
** Cpu_OnNMIINT - void Cpu_OnNMIINT(void);
**
** ###################################################################*/
/*!
** @file Events.c
** @version 01.00
** @brief
** This is user's event module.
** Put your event handler code here.
*/
/*!
** @addtogroup Events_module Events module documentation
** @{
*/
/* MODULE Events */
#include "Cpu.h"
#include "Events.h"
#ifdef __cplusplus
extern "C" {
#endif
/* User includes (#include below this line is not maintained by Processor Expert) */
extern byte flag;
extern byte flag2;
//extern bool ValuesAvailable;
//extern byte Values[3];
//extern int SysTicks;
//extern int msg_sent_flag;
//extern int msg_received_flag;
/*
** ===================================================================
** Event : Cpu_OnNMIINT (module Events)
**
** Component : Cpu [MKL25Z128LK4]
*/
/*!
** @brief
** This event is called when the Non maskable interrupt had
** occurred. This event is automatically enabled when the [NMI
** interrupt] property is set to 'Enabled'.
*/
/* ===================================================================*/
void Cpu_OnNMIINT(void)
{
/* Write your code here ... */
}
/*
** ===================================================================
** Event : AS1_OnError (module Events)
**
** Component : AS1 [AsynchroSerial]
** Description :
** This event is called when a channel error (not the error
** returned by a given method) occurs. The errors can be read
** using <GetError> method.
** The event is available only when the <Interrupt
** service/event> property is enabled.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void AS1_OnError(void)
{
}
/*
** ===================================================================
** Event : AS1_OnTxChar (module Events)
**
** Component : AS1 [AsynchroSerial]
** Description :
** This event is called after a character is transmitted.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void AS1_OnTxChar(void)
{
/* Write your code here ... */
}
/*
** ===================================================================
** Event : AS1_OnFullRxBuf (module Events)
**
** Component : AS1 [AsynchroSerial]
** Description :
** This event is called when the input buffer is full;
** i.e. after reception of the last character
** that was successfully placed into input buffer.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void AS1_OnFullRxBuf(void)
{
}
/*
** ===================================================================
** Event : AS1_OnFreeTxBuf (module Events)
**
** Component : AS1 [AsynchroSerial]
** Description :
** This event is called after the last character in output
** buffer is transmitted.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void AS1_OnFreeTxBuf(void)
{
// msg_sent_flag = 1;
}
/*
** ===================================================================
** Event : TI1_OnInterrupt (module Events)
**
** Component : TI1 [TimerInt]
** Description :
** When a timer interrupt occurs this event is called (only
** when the component is enabled - <Enable> and the events are
** enabled - <EnableEvent>). This event is enabled only if a
** <interrupt service/event> is enabled.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void TI1_OnInterrupt(void)
{
// SysTicks++;
}
/*
** ===================================================================
** Event : CI2C1_OnReceiveData (module Events)
**
** Component : CI2C1 [InternalI2C]
** Description :
** This event is invoked when I2C finishes the reception of the
** data successfully. This event is not available for the SLAVE
** mode and if both RecvChar and RecvBlock are disabled. This
** event is enabled only if interrupts/events are enabled.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void CI2C1_OnReceiveData(void)
{
flag2 = 1;
}
/*
** ===================================================================
** Event : AS1_OnRxChar (module Events)
**
** Component : AS1 [AsynchroSerial]
** Description :
** This event is called after a correct character is received.
** The event is available only when the <Interrupt
** service/event> property is enabled and either the <Receiver>
** property is enabled or the <SCI output mode> property (if
** supported) is set to Single-wire mode.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void AS1_OnRxChar(void)
{
// msg_received_flag = 1;
}
/*
** ===================================================================
** Event : CI2C1_OnTransmitData (module Events)
**
** Component : CI2C1 [InternalI2C]
** Description :
** This event is invoked when I2C finishes the transmission of
** the data successfully. This event is not available for the
** SLAVE mode and if both SendChar and SendBlock are disabled.
** This event is enabled only if interrupts/events are enabled.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void CI2C1_OnTransmitData(void)
{
flag = 1;
}
/*
** ===================================================================
** Event : AD1_OnEnd (module Events)
**
** Component : AD1 [ADC]
** Description :
** This event is called after the measurement (which consists
** of <1 or more conversions>) is/are finished.
** The event is available only when the <Interrupt
** service/event> property is enabled.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void AD1_OnEnd(void)
{
// AD1_GetValue16((word *)Values); // Get AD conversion results
// ValuesAvailable = TRUE; // set control variable
}
/*
** ===================================================================
** Event : AD1_OnCalibrationEnd (module Events)
**
** Component : AD1 [ADC]
** Description :
** This event is called when the calibration has been finished.
** User should check if the calibration pass or fail by
** Calibration status method./nThis event is enabled only if
** the <Interrupt service/event> property is enabled.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
void AD1_OnCalibrationEnd(void)
{
/* Write your code here ... */
}
/* END Events */
#ifdef __cplusplus
} /* extern "C" */
#endif
/*!
** @}
*/
/*
** ###################################################################
**
** This file was created by Processor Expert 10.3 [05.09]
** for the Freescale Kinetis series of microcontrollers.
**
** ###################################################################
*/
SHIELD OLED Library Header File for Navigational System Code (KL25Z)
SHIELD OLED Library Header File for Navigational System Code (KL25Z)
#ifndef Shield_OLED_display_H_
#define Shield_OLED_display_H_
//#include "common.h"
#include <stdint.h>
/**************Defines Here************************/
/*
** WHO_AM_I Device ID Register
*/
#define OLED_I2C_SLAVE_ID 0x3C
#define SYSMOD 0x0B
// ***** ssd Commands and data *************
/* OLED (ssd1306_ Commands and data */
#define SSD1306_LCDWIDTH 128
#define SSD1306_LCDHEIGHT 64
#define SSD1306_SETCONTRAST 0x81
#define SSD1306_DISPLAYALLON_RESUME 0xA4
#define SSD1306_DISPLAYALLON 0xA5
#define SSD1306_NORMALDISPLAY 0xA6
#define SSD1306_INVERTDISPLAY 0xA7
#define SSD1306_DISPLAYOFF 0xAE
#define SSD1306_DISPLAYON 0xAF
#define SSD1306_SETDISPLAYOFFSET 0xD3
#define SSD1306_SETCOMPINS 0xDA
#define SSD1306_SETVCOMDETECT 0xDB
#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
#define SSD1306_SETPRECHARGE 0xD9
#define SSD1306_SETMULTIPLEX 0xA8
#define SSD1306_SETLOWCOLUMN 0x00
#define SSD1306_SETHIGHCOLUMN 0x10
#define SSD1306_SETSTARTLINE 0x40
#define SSD1306_MEMORYMODE 0x20
#define SSD1306_COLUMNADDR 0x21
#define SSD1306_PAGEADDR 0x22
#define SSD1306_COMSCANINC 0xC0
#define SSD1306_COMSCANDEC 0xC8
#define SSD1306_SEGREMAP 0xA0
#define SSD1306_CHARGEPUMP 0x8D
#define SSD1306_EXTERNALVCC 0x1
#define SSD1306_SWITCHCAPVCC 0x2
void OLED_command(uint8_t);
void OLED_data(uint8_t);
void OLED_setColAddress(uint16_t);
void OLED_setPageAddress(uint16_t);
void OLED_transferBuffer(uint8_t *, uint16_t ,uint8_t, uint8_t);
void OLED_initializeDisplay(void);
void OLED_printChar(char);
void OLED_printChar_no_spacing(char);
void OLED_clearDisplay(void);
void OLED_setLocation(uint16_t,uint16_t);
void OLED_writeString (char *);
#endif //Shield_OLED_display_H_
SHIELD OLED Library Source File for Navigational System Code (KL25Z)
SHIELD OLED Library Source File for Navigational System Code (KL25Z)
#include <stdint.h>
#include "Shield_OLED_display.h"
#include "my_i2c.h"
/**************Global Variables Here************************/
/* Dot Matrix of all ASCII characters
* Nonprinting characters are symbols
* Each character in the display is a matrix of points, 5x8
* The lower row of points is always all off.
* The first column is the first element in the subarray, MSB at the bottom of the column
* Example, the pattern for a '0' is the element with values
* 0x3E, 0x51, 0x49, 0x45, 0x3E, map these binary values vertically and you get an ascii 0
*
* - x x x - LSB
* x - - - x
* x - - x x
* x - x - x
* x x - - x
* x - - - x
* - x x x -
* - - - - - MSB
* | | | | |
* | | | | ---0x3E
* | | | -------0x45
* | | -----------0x49
* | ---------------0x51
* -------------------0x3E
*
*
*/
/* For the Extended ASCII character set, 128 and above, the symbols in the
* table look to be standard ones which can be found at http:\\www.asciitable.com
*
*/
uint8_t asciiChar[256][5] = {
{0x00, 0x00, 0x00, 0x00, 0x00}, // NUL
{0x3E, 0x5B, 0x4F, 0x5B, 0x3E}, // SOH
{0x3E, 0x6B, 0x4F, 0x6B, 0x3E}, //
{0x1C, 0x3E, 0x7C, 0x3E, 0x1C}, //
{0x18, 0x3C, 0x7E, 0x3C, 0x18}, //
{0x1C, 0x57, 0x7D, 0x57, 0x1C}, //
{0x1C, 0x5E, 0x7F, 0x5E, 0x1C}, //
{0x00, 0x18, 0x3C, 0x18, 0x00}, //
{0xFF, 0xE7, 0xC3, 0xE7, 0xFF}, //
{0x00, 0x18, 0x24, 0x18, 0x00}, //
{0xFF, 0xE7, 0xDB, 0xE7, 0xFF}, //
{0x30, 0x48, 0x3A, 0x06, 0x0E}, //
{0x26, 0x29, 0x79, 0x29, 0x26}, //
{0x40, 0x7F, 0x05, 0x05, 0x07}, //
{0x40, 0x7F, 0x05, 0x25, 0x3F}, //
{0x5A, 0x3C, 0xE7, 0x3C, 0x5A}, //
{0x7F, 0x3E, 0x1C, 0x1C, 0x08}, //
{0x08, 0x1C, 0x1C, 0x3E, 0x7F}, //
{0x14, 0x22, 0x7F, 0x22, 0x14}, //
{0x5F, 0x5F, 0x00, 0x5F, 0x5F}, //
{0x06, 0x09, 0x7F, 0x01, 0x7F}, //
{0x00, 0x66, 0x89, 0x95, 0x6A}, //
{0x60, 0x60, 0x60, 0x60, 0x60}, //
{0x94, 0xA2, 0xFF, 0xA2, 0x94}, //
{0x08, 0x04, 0x7E, 0x04, 0x08}, //
{0x10, 0x20, 0x7E, 0x20, 0x10}, //
{0x08, 0x08, 0x2A, 0x1C, 0x08}, //
{0x08, 0x1C, 0x2A, 0x08, 0x08}, //
{0x1E, 0x10, 0x10, 0x10, 0x10}, //
{0x0C, 0x1E, 0x0C, 0x1E, 0x0C}, //
{0x30, 0x38, 0x3E, 0x38, 0x30}, //
{0x06, 0x0E, 0x3E, 0x0E, 0x06}, //
{0x00, 0x00, 0x00, 0x00, 0x00}, // space
{0x00, 0x00, 0x5F, 0x00, 0x00}, // !
{0x00, 0x07, 0x00, 0x07, 0x00}, // "
{0x14, 0x7F, 0x14, 0x7F, 0x14}, // #
{0x24, 0x2A, 0x7F, 0x2A, 0x12}, //
{0x23, 0x13, 0x08, 0x64, 0x62}, // %
{0x36, 0x49, 0x56, 0x20, 0x50}, // &
{0x00, 0x08, 0x07, 0x03, 0x00}, // '
{0x00, 0x1C, 0x22, 0x41, 0x00}, //
{0x00, 0x41, 0x22, 0x1C, 0x00}, //
{0x2A, 0x1C, 0x7F, 0x1C, 0x2A}, //
{0x08, 0x08, 0x3E, 0x08, 0x08}, //
{0x00, 0x80, 0x70, 0x30, 0x00}, //
{0x08, 0x08, 0x08, 0x08, 0x08}, //
{0x00, 0x00, 0x60, 0x60, 0x00}, //
{0x20, 0x10, 0x08, 0x04, 0x02}, //
{0x3E, 0x51, 0x49, 0x45, 0x3E}, // 0
{0x00, 0x42, 0x7F, 0x40, 0x00}, // 1
{0x72, 0x49, 0x49, 0x49, 0x46}, // 2
{0x21, 0x41, 0x49, 0x4D, 0x33}, // 3
{0x18, 0x14, 0x12, 0x7F, 0x10}, // 4
{0x27, 0x45, 0x45, 0x45, 0x39}, // 5
{0x3C, 0x4A, 0x49, 0x49, 0x31}, // 6
{0x41, 0x21, 0x11, 0x09, 0x07}, // 7
{0x36, 0x49, 0x49, 0x49, 0x36}, // 8
{0x46, 0x49, 0x49, 0x29, 0x1E}, // 9
{0x00, 0x00, 0x14, 0x00, 0x00}, //
{0x00, 0x40, 0x34, 0x00, 0x00}, //
{0x00, 0x08, 0x14, 0x22, 0x41}, //
{0x14, 0x14, 0x14, 0x14, 0x14}, //
{0x00, 0x41, 0x22, 0x14, 0x08}, //
{0x02, 0x01, 0x59, 0x09, 0x06}, //
{0x3E, 0x41, 0x5D, 0x59, 0x4E}, //
{0x7C, 0x12, 0x11, 0x12, 0x7C}, //
{0x7F, 0x49, 0x49, 0x49, 0x36}, //
{0x3E, 0x41, 0x41, 0x41, 0x22}, //
{0x7F, 0x41, 0x41, 0x41, 0x3E}, //
{0x7F, 0x49, 0x49, 0x49, 0x41}, //
{0x7F, 0x09, 0x09, 0x09, 0x01}, //
{0x3E, 0x41, 0x41, 0x51, 0x73}, //
{0x7F, 0x08, 0x08, 0x08, 0x7F}, //
{0x00, 0x41, 0x7F, 0x41, 0x00}, //
{0x20, 0x40, 0x41, 0x3F, 0x01}, //
{0x7F, 0x08, 0x14, 0x22, 0x41}, //
{0x7F, 0x40, 0x40, 0x40, 0x40}, //
{0x7F, 0x02, 0x1C, 0x02, 0x7F}, //
{0x7F, 0x04, 0x08, 0x10, 0x7F}, //
{0x3E, 0x41, 0x41, 0x41, 0x3E}, //
{0x7F, 0x09, 0x09, 0x09, 0x06}, //
{0x3E, 0x41, 0x51, 0x21, 0x5E}, //
{0x7F, 0x09, 0x19, 0x29, 0x46}, //
{0x26, 0x49, 0x49, 0x49, 0x32}, //
{0x03, 0x01, 0x7F, 0x01, 0x03}, //
{0x3F, 0x40, 0x40, 0x40, 0x3F}, //
{0x1F, 0x20, 0x40, 0x20, 0x1F}, //
{0x3F, 0x40, 0x38, 0x40, 0x3F}, //
{0x63, 0x14, 0x08, 0x14, 0x63}, //
{0x03, 0x04, 0x78, 0x04, 0x03}, //
{0x61, 0x59, 0x49, 0x4D, 0x43}, //
{0x00, 0x7F, 0x41, 0x41, 0x41}, //
{0x02, 0x04, 0x08, 0x10, 0x20}, //
{0x00, 0x41, 0x41, 0x41, 0x7F}, //
{0x04, 0x02, 0x01, 0x02, 0x04}, //
{0x40, 0x40, 0x40, 0x40, 0x40}, //
{0x00, 0x03, 0x07, 0x08, 0x00}, //
{0x20, 0x54, 0x54, 0x78, 0x40}, //
{0x7F, 0x28, 0x44, 0x44, 0x38}, //
{0x38, 0x44, 0x44, 0x44, 0x28}, //
{0x38, 0x44, 0x44, 0x28, 0x7F}, //
{0x38, 0x54, 0x54, 0x54, 0x18}, //
{0x00, 0x08, 0x7E, 0x09, 0x02}, //
{0x18, 0xA4, 0xA4, 0x9C, 0x78}, //
{0x7F, 0x08, 0x04, 0x04, 0x78}, //
{0x00, 0x44, 0x7D, 0x40, 0x00},
{0x20, 0x40, 0x40, 0x3D, 0x00},
{0x7F, 0x10, 0x28, 0x44, 0x00},
{0x00, 0x41, 0x7F, 0x40, 0x00},
{0x7C, 0x04, 0x78, 0x04, 0x78},
{0x7C, 0x08, 0x04, 0x04, 0x78},
{0x38, 0x44, 0x44, 0x44, 0x38},
{0xFC, 0x18, 0x24, 0x24, 0x18},
{0x18, 0x24, 0x24, 0x18, 0xFC},
{0x7C, 0x08, 0x04, 0x04, 0x08},
{0x48, 0x54, 0x54, 0x54, 0x24},
{0x04, 0x04, 0x3F, 0x44, 0x24},
{0x3C, 0x40, 0x40, 0x20, 0x7C},
{0x1C, 0x20, 0x40, 0x20, 0x1C},
{0x3C, 0x40, 0x30, 0x40, 0x3C},
{0x44, 0x28, 0x10, 0x28, 0x44},
{0x4C, 0x90, 0x90, 0x90, 0x7C},
{0x44, 0x64, 0x54, 0x4C, 0x44},
{0x00, 0x08, 0x36, 0x41, 0x00},
{0x00, 0x00, 0x77, 0x00, 0x00},
{0x00, 0x41, 0x36, 0x08, 0x00},
{0x02, 0x01, 0x02, 0x04, 0x02},
{0x3C, 0x26, 0x23, 0x26, 0x3C},
{0x1E, 0xA1, 0xA1, 0x61, 0x12},
{0x3A, 0x40, 0x40, 0x20, 0x7A},
{0x38, 0x54, 0x54, 0x55, 0x59},
{0x21, 0x55, 0x55, 0x79, 0x41},
{0x22, 0x54, 0x54, 0x78, 0x42},
{0x21, 0x55, 0x54, 0x78, 0x40},
{0x20, 0x54, 0x55, 0x79, 0x40},
{0x0C, 0x1E, 0x52, 0x72, 0x12},
{0x39, 0x55, 0x55, 0x55, 0x59},
{0x39, 0x54, 0x54, 0x54, 0x59},
{0x39, 0x55, 0x54, 0x54, 0x58},
{0x00, 0x00, 0x45, 0x7C, 0x41},
{0x00, 0x02, 0x45, 0x7D, 0x42},
{0x00, 0x01, 0x45, 0x7C, 0x40},
{0x7D, 0x12, 0x11, 0x12, 0x7D},
{0xF0, 0x28, 0x25, 0x28, 0xF0},
{0x7C, 0x54, 0x55, 0x45, 0x00},
{0x20, 0x54, 0x54, 0x7C, 0x54},
{0x7C, 0x0A, 0x09, 0x7F, 0x49},
{0x32, 0x49, 0x49, 0x49, 0x32},
{0x3A, 0x44, 0x44, 0x44, 0x3A},
{0x32, 0x4A, 0x48, 0x48, 0x30},
{0x3A, 0x41, 0x41, 0x21, 0x7A},
{0x3A, 0x42, 0x40, 0x20, 0x78},
{0x00, 0x9D, 0xA0, 0xA0, 0x7D},
{0x3D, 0x42, 0x42, 0x42, 0x3D},
{0x3D, 0x40, 0x40, 0x40, 0x3D},
{0x3C, 0x24, 0xFF, 0x24, 0x24},
{0x48, 0x7E, 0x49, 0x43, 0x66},
{0x2B, 0x2F, 0xFC, 0x2F, 0x2B},
{0xFF, 0x09, 0x29, 0xF6, 0x20},
{0xC0, 0x88, 0x7E, 0x09, 0x03},
{0x20, 0x54, 0x54, 0x79, 0x41},
{0x00, 0x00, 0x44, 0x7D, 0x41},
{0x30, 0x48, 0x48, 0x4A, 0x32},
{0x38, 0x40, 0x40, 0x22, 0x7A},
{0x00, 0x7A, 0x0A, 0x0A, 0x72},
{0x7D, 0x0D, 0x19, 0x31, 0x7D},
{0x26, 0x29, 0x29, 0x2F, 0x28},
{0x26, 0x29, 0x29, 0x29, 0x26},
{0x30, 0x48, 0x4D, 0x40, 0x20},
{0x38, 0x08, 0x08, 0x08, 0x08},
{0x08, 0x08, 0x08, 0x08, 0x38},
{0x2F, 0x10, 0xC8, 0xAC, 0xBA},
{0x2F, 0x10, 0x28, 0x34, 0xFA},
{0x00, 0x00, 0x7B, 0x00, 0x00},
{0x08, 0x14, 0x2A, 0x14, 0x22},
{0x22, 0x14, 0x2A, 0x14, 0x08},
{0x55, 0x00, 0x55, 0x00, 0x55},
{0xAA, 0x55, 0xAA, 0x55, 0xAA},
{0xFF, 0x55, 0xFF, 0x55, 0xFF},
{0x00, 0x00, 0x00, 0xFF, 0x00},
{0x10, 0x10, 0x10, 0xFF, 0x00},
{0x14, 0x14, 0x14, 0xFF, 0x00},
{0x10, 0x10, 0xFF, 0x00, 0xFF},
{0x10, 0x10, 0xF0, 0x10, 0xF0},
{0x14, 0x14, 0x14, 0xFC, 0x00},
{0x14, 0x14, 0xF7, 0x00, 0xFF},
{0x00, 0x00, 0xFF, 0x00, 0xFF},
{0x14, 0x14, 0xF4, 0x04, 0xFC},
{0x14, 0x14, 0x17, 0x10, 0x1F},
{0x10, 0x10, 0x1F, 0x10, 0x1F},
{0x14, 0x14, 0x14, 0x1F, 0x00},
{0x10, 0x10, 0x10, 0xF0, 0x00},
{0x00, 0x00, 0x00, 0x1F, 0x10},
{0x10, 0x10, 0x10, 0x1F, 0x10},
{0x10, 0x10, 0x10, 0xF0, 0x10},
{0x00, 0x00, 0x00, 0xFF, 0x10},
{0x10, 0x10, 0x10, 0x10, 0x10},
{0x10, 0x10, 0x10, 0xFF, 0x10},
{0x00, 0x00, 0x00, 0xFF, 0x14},
{0x00, 0x00, 0xFF, 0x00, 0xFF},
{0x00, 0x00, 0x1F, 0x10, 0x17},
{0x00, 0x00, 0xFC, 0x04, 0xF4},
{0x14, 0x14, 0x17, 0x10, 0x17},
{0x14, 0x14, 0xF4, 0x04, 0xF4},
{0x00, 0x00, 0xFF, 0x00, 0xF7},
{0x14, 0x14, 0x14, 0x14, 0x14},
{0x14, 0x14, 0xF7, 0x00, 0xF7},
{0x14, 0x14, 0x14, 0x17, 0x14},
{0x10, 0x10, 0x1F, 0x10, 0x1F},
{0x14, 0x14, 0x14, 0xF4, 0x14},
{0x10, 0x10, 0xF0, 0x10, 0xF0},
{0x00, 0x00, 0x1F, 0x10, 0x1F},
{0x00, 0x00, 0x00, 0x1F, 0x14},
{0x00, 0x00, 0x00, 0xFC, 0x14},
{0x00, 0x00, 0xF0, 0x10, 0xF0},
{0x10, 0x10, 0xFF, 0x10, 0xFF},
{0x14, 0x14, 0x14, 0xFF, 0x14},
{0x10, 0x10, 0x10, 0x1F, 0x00},
{0x00, 0x00, 0x00, 0xF0, 0x10},
{0xFF, 0xFF, 0xFF, 0xFF, 0xFF},
{0xF0, 0xF0, 0xF0, 0xF0, 0xF0},
{0xFF, 0xFF, 0xFF, 0x00, 0x00},
{0x00, 0x00, 0x00, 0xFF, 0xFF},
{0x0F, 0x0F, 0x0F, 0x0F, 0x0F},
{0x38, 0x44, 0x44, 0x38, 0x44},
{0xFC, 0x4A, 0x4A, 0x4A, 0x34},
{0x7E, 0x02, 0x02, 0x06, 0x06},
{0x02, 0x7E, 0x02, 0x7E, 0x02},
{0x63, 0x55, 0x49, 0x41, 0x63},
{0x38, 0x44, 0x44, 0x3C, 0x04},
{0x40, 0x7E, 0x20, 0x1E, 0x20},
{0x06, 0x02, 0x7E, 0x02, 0x02},
{0x99, 0xA5, 0xE7, 0xA5, 0x99},
{0x1C, 0x2A, 0x49, 0x2A, 0x1C},
{0x4C, 0x72, 0x01, 0x72, 0x4C},
{0x30, 0x4A, 0x4D, 0x4D, 0x30},
{0x30, 0x48, 0x78, 0x48, 0x30},
{0xBC, 0x62, 0x5A, 0x46, 0x3D},
{0x3E, 0x49, 0x49, 0x49, 0x00},
{0x7E, 0x01, 0x01, 0x01, 0x7E},
{0x2A, 0x2A, 0x2A, 0x2A, 0x2A},
{0x44, 0x44, 0x5F, 0x44, 0x44},
{0x40, 0x51, 0x4A, 0x44, 0x40},
{0x40, 0x44, 0x4A, 0x51, 0x40},
{0x00, 0x00, 0xFF, 0x01, 0x03},
{0xE0, 0x80, 0xFF, 0x00, 0x00},
{0x08, 0x08, 0x6B, 0x6B, 0x08},
{0x36, 0x12, 0x36, 0x24, 0x36},
{0x06, 0x0F, 0x09, 0x0F, 0x06},
{0x00, 0x00, 0x18, 0x18, 0x00},
{0x00, 0x00, 0x10, 0x10, 0x00},
{0x30, 0x40, 0xFF, 0x01, 0x01},
{0x00, 0x1F, 0x01, 0x01, 0x1E},
{0xF0, 0xF0, 0xF0, 0xF0, 0xF0},
{0x0F, 0x0F, 0x0F, 0x0F, 0x0F},
{0xFF, 0xFF, 0xFF, 0xFF, 0xFF}
};
void OLED_command(uint8_t c)
{
uint8_t control = 0x00; // some use 0X00 other examples use 0X80. I tried both
I2C0WriteRegister(OLED_I2C_SLAVE_ID, control, c);
}
/* write a single 8 bit high column */
void OLED_data(uint8_t column)
{
I2C0WriteRegister(OLED_I2C_SLAVE_ID, 0x40, column);
}
// Used when doing Horizontal or Vertical Addressing
void OLED_setColAddress(uint16_t xCol)
{
if(xCol>(SSD1306_LCDWIDTH-1))return;
OLED_command(SSD1306_COLUMNADDR);
OLED_command(xCol); // Column start address
OLED_command(SSD1306_LCDWIDTH-1); // Column end address 127
}
// Used when doing Horizontal or Vertical Addressing
void OLED_setPageAddress(uint16_t yPage)
{ if(yPage>(SSD1306_LCDHEIGHT/8-1))return;
OLED_command(SSD1306_PAGEADDR);
OLED_command(yPage); // Starting page address 0-7
OLED_command((SSD1306_LCDHEIGHT/8)-1); // Ending Page address 7
}
// Transfers data to the CGRAM in the SSD1306 using a pointer and size
void OLED_transferBuffer(uint8_t *p, uint16_t size, uint8_t xCol, uint8_t yPage)
{
uint16_t j;
OLED_setColAddress(xCol);
OLED_setPageAddress(yPage);
for(j = 0; j < size; j++){
I2C0WriteRegister(OLED_I2C_SLAVE_ID, 0x40, *p);
p++;
}
}
void OLED_clearDisplay(void)
{
uint16_t j;
// set the Column and Page to 0,0
OLED_setColAddress(0);
OLED_setPageAddress(0);
/* write out all 0s */
for(j = 0; j < 1024; j++){
I2C0WriteRegister(OLED_I2C_SLAVE_ID, 0x40, 0);
}
// set location back to 0. 0
OLED_setColAddress(0);
OLED_setPageAddress(0);
}
// Set screen location of the column (0 to 127) and page (0 to 7)
void OLED_setLocation(uint16_t xCol, uint16_t yPage){
OLED_setColAddress(xCol);
OLED_setPageAddress(yPage);
}
/* Sends out an ASCII character using a 5x8 dot matrix for characters
* The char parameter (which is signed) is cast to an unsigned char when
* pulling the element from the ASCII array. This means values 0 to 127
* of the char parameter are 0 to 127 in the array.
* However, values of -1 to -128 of the char parameter are values
* 255 to 128 in the array. In other words the char value (c) for an array
* subscript (x) in the range 128 to 255 is c = x - 256 and x = 256 + c
*/
void OLED_printChar(char character)
{
uint8_t column;
I2C0WriteRegister(OLED_I2C_SLAVE_ID, 0x40, 0); //some space before each character
for(column = 0; column < 5; column++){
I2C0WriteRegister(OLED_I2C_SLAVE_ID, 0x40, asciiChar[(unsigned char)character][column]);
}
I2C0WriteRegister(OLED_I2C_SLAVE_ID, 0x40, 0); //some space after each character
}
void OLED_printChar_no_spacing(char character)
{
uint8_t column;
for(column = 0; column < 5; column++){
I2C0WriteRegister(OLED_I2C_SLAVE_ID, 0x40, asciiChar[(unsigned char)character][column]); //Modified to display characters from my Char Array
}
}
// init according to SSD1306 data sheet and many places on the web
void OLED_initializeDisplay()
{
init_I2C0(); // C8 (SCL) C9 (SDA) are the oled
// Init sequence for 128x64 OLED module
OLED_command(SSD1306_DISPLAYOFF); // 0xAE
OLED_command(SSD1306_SETDISPLAYCLOCKDIV); // 0xD5
OLED_command(0x80); // the suggested ratio 0x80
OLED_command(SSD1306_SETMULTIPLEX); // 0xA8
OLED_command(0x3F);
OLED_command(SSD1306_SETDISPLAYOFFSET); // 0xD3
OLED_command(0x0); // no offset
OLED_command(SSD1306_SETSTARTLINE);// | 0x0); // line #0
OLED_command(SSD1306_CHARGEPUMP); // 0x8D
OLED_command(0x14); // using internal VCC
OLED_command(SSD1306_MEMORYMODE); // 0x20
OLED_command(0x00); // 0x00 horizontal addressing
OLED_command(SSD1306_SEGREMAP | 0x1); // rotate screen 180
OLED_command(SSD1306_COMSCANDEC); // rotate screen 180
OLED_command(SSD1306_SETCOMPINS); // 0xDA
OLED_command(0x12);
OLED_command(SSD1306_SETCONTRAST); // 0x81
OLED_command(0xCF);
OLED_command(SSD1306_SETPRECHARGE); // 0xd9
OLED_command(0xF1);
OLED_command(SSD1306_SETVCOMDETECT); // 0xDB
OLED_command(0x40);
OLED_command(SSD1306_DISPLAYALLON_RESUME); // 0xA4
OLED_command(SSD1306_NORMALDISPLAY); // 0xA6
OLED_command(SSD1306_DISPLAYON); //switch on OLED
}
/* writes a character string using a pointer and NULL at the end */
void OLED_writeString(char *c)
{
while (*c){
OLED_printChar(*c);
c++;
}
}
Page updated
Google Sites
Report abuse