supleed2/ELEC50003-P1-CW
1
0
Fork 0
mirror of https://github.com/supleed2/ELEC50003-P1-CW.git synced 2024-12-22 21:45:49 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into Integration

Browse source
This commit is contained in:
Aadi Desai 2021-06-14 12:17:20 +01:00 committed by GitHub
parent 5dba48bee3 ddea9f26ca
commit 54d30741e0
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
9 changed files with 622 additions and 333 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
Control/.gitignore vendored
View file

@ -3,4 +3,4 @@
.vscode/c_cpp_properties.json .vscode/c_cpp_properties.json
.vscode/launch.json .vscode/launch.json
.vscode/ipch .vscode/ipch
src/credentials.h include/credentials.h

0
Control/src/credentials.h.dummy → Control/include/credentials.h.dummy
View file

21
Control/include/instruction.h Normal file
View file

@ -0,0 +1,21 @@
#ifndef INSTRUCTION_H
#define INSTRUCTION_H
typedef enum {
INSTR_RESET = -1,
INSTR_STOP,
INSTR_MOVE,
INSTR_CHARGE
} instr_t;
typedef struct instruction
{
int id;
int instr;
int heading;
int distance;
float speed;
int charge;
} RoverInstruction;
#endif

11
Control/include/status.h Normal file
View file

@ -0,0 +1,11 @@
#ifndef CONTROL_STATUS_H
#define CONTROL_STATUS_H
typedef enum {
CS_ERROR = -1,
CS_IDLE,
CS_MOVING,
CS_CHARGING
} ControlStatus_t;
#endif

11
Control/ref/command.cpp
View file

@ -3,8 +3,10 @@
#include <string> #include <string>
#define WebSocket 0 #define WebSocket 0
int batteryVoltage, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Command ==> Control int state, totalTripDistance, currentHeading, current_x, current_y, signal_strength, lastCompletedCommand_id; // Info Control ==> Command
int command_id, mode, reqHeading, reqDistance, reqSpeed, reqCharge; // Info Control ==> Command float batteryVoltage, batteryLevel, batteryCycles; // Info Control ==> Command
int command_id, mode, reqHeading, reqDistance, reqCharge; // Info Command ==> Control
float reqSpeed; // Info Command ==> Control
void setup() {} void setup() {}
@ -12,7 +14,10 @@ void loop()
{ {
DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
deserializeJson(rdoc, WebSocket); // Take JSON input from WebSocket deserializeJson(rdoc, WebSocket); // Take JSON input from WebSocket
batteryVoltage = rdoc["bV"]; state = rdoc["st"]; // State: -1 = Error, 0 = Idle, 1 = Moving, 2 = Charging
batteryVoltage = rdoc["bV"];
batteryLevel = rdoc["bL"];
batteryCycles = rdoc["bC"];
totalTripDistance = rdoc["tD"]; totalTripDistance = rdoc["tD"];
currentHeading = rdoc["cH"]; currentHeading = rdoc["cH"];
current_x = rdoc["pos"][0]; current_x = rdoc["pos"][0];

5
Control/ref/drive.cpp
View file

@ -18,9 +18,10 @@ void loop()
DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
deserializeJson(rdoc, Serial1); // Take JSON input from UART1 deserializeJson(rdoc, Serial1); // Take JSON input from UART1
int requiredHeading = rdoc["rH"]; // if -1: command in progress, returning requested heading, dist/sp to be ignored int requiredHeading = rdoc["rH"]; // if -1: command in progress, returning requested heading, dist/sp to be ignored
int distance = rdoc["dist"]; int distance = rdoc["dist"]; // -1 for emergency stop
float spd = rdoc["sp"]; float spd = rdoc["sp"]; // -1 for emergency stop
int currentHeading = rdoc["cH"]; int currentHeading = rdoc["cH"];
bool resetDistanceTravelled = rdoc["rstD"];
bool commandComplete = 0; bool commandComplete = 0;
float powerUsage_mWh = 0.0; float powerUsage_mWh = 0.0;

48
Control/ref/vision.cpp
View file

@ -1 +1,49 @@
#include <Arduino.h>
#include <ArduinoJson.h>
#include <string>
const int ARDUINO_IO[16] = {-1/*RX*/, -1/*RX*/, 23, 22, 21, 19, 18, 5, 17, 16, 14, 4, 15, 2, 13, 12}; // Expansion board mapping const int ARDUINO_IO[16] = {-1/*RX*/, -1/*RX*/, 23, 22, 21, 19, 18, 5, 17, 16, 14, 4, 15, 2, 13, 12}; // Expansion board mapping
#define RXpin ARDUINO_IO[11] // Define your RX pin here
#define TXpin ARDUINO_IO[10] // Define your TX pin here
FILE* SerialUART;
void setup()
{
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
}
void loop()
{
int command = getc(SerialUART);
// command char, used for controlling exposure/focus/gain settings:
// e = increase exposure
// d = decrease exposure
// r = increase focus
// f = decrease focus
// t = increase gain
// g = decrease gain
// Bounding Box edges
int bb_left = 0;
int bb_right = 0;
int bb_top = 0;
int bb_bottom = 0;
// Weighted average of detected pixels coordinates
int centre_x = 0;
int centre_y = 0;
// Heading from DE10-Lite magnetometer
float heading = 0.0;
// Build hardcode JSON packet on DE10-Lite using fprintf() as space is minimal and library would be too large.
// fprintf(SerialUART, "{\"bb\":[%d,%d,%d,%d],\"cen\":[%d,%d],\"cH\":%d\"}", bb_left, bb_right, bb_top, bb_bottom, centre_x, centre_y, heading);
DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
deserializeJson(rdoc, SerialUART);
bb_left = rdoc["bb"][0];
bb_right = rdoc["bb"][1];
bb_top = rdoc["bb"][2];
bb_bottom = rdoc["bb"][3];
centre_x = rdoc["cen"][0];
centre_y = rdoc["cen"][1];
heading = rdoc["cH"];
}

409
Control/src/main.cpp
View file

@ -1,37 +1,73 @@
#pragma region Includes
#include <Arduino.h> #include <Arduino.h>
#include <string> #include <string>
#include <SoftwareSerial.h> // Software Serial not currently needed #include <SoftwareSerial.h> // Software Serial not currently needed
#include <SoftwareSerial.h>
#include <AsyncTCP.h> #include <AsyncTCP.h>
#include <ESPAsyncWebServer.h> #include <ESPAsyncWebServer.h>
#include <ESPmDNS.h> #include <ESPmDNS.h>
#include "TickerV2.h" #include "TickerV2.h"
#include <WebSocketsServer.h> #include <WebSocketsServer.h>
#include <credentials.h> #include "credentials.h"
#include <ArduinoJson.h> #include <ArduinoJson.h>
#include <SPIFFS.h> #include <SPIFFS.h>
#include "status.h"
#include "instruction.h"
#include <queue>
#pragma endregion
// Enable extra debugging info #pragma region Enable extra debugging info for ESP32
#undef LOG_LOCAL_LEVEL #undef LOG_LOCAL_LEVEL
#define LOG_LOCAL_LEVEL ESP_LOG_VERBOSE #define LOG_LOCAL_LEVEL ESP_LOG_VERBOSE
#include "esp_log.h" #include "esp_log.h"
#pragma endregion
#define RX1pin 14 // Pin 10 on expansion board, UART1 #pragma region Definitions eg pins
#define TX1pin 4 // Pin 11 on expansion board, UART1 #define RX1pin 18 // Pin 6 on expansion board, UART1
#define TX1pin 5 // Pin 7 on expansion board, UART1
#define RX2pin 17 // Pin 8 on expansion board, UART2
#define TX2pin 16 // Pin 9 on expansion board, UART2
#define RX3pin 14 // Pin 10 on expansion board, UART3
#define TX3pin 4 // Pin 11 on expansion board, UART3
#pragma endregion
// Function Declarations #pragma region Function Declarations
void printFPGAoutput();
void returnSensorData();
void notFound(AsyncWebServerRequest *request); void notFound(AsyncWebServerRequest *request);
void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length); void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length);
void queueInstruction(RoverInstruction instruction);
void sendToCommand();
void sendToDrive(RoverInstruction instruction);
void recvFromDrive();
void sendToEnergy(bool instruction);
void recvFromEnergy();
void sendToVision();
void recvFromVision();
void emergencyStop();
#pragma endregion
// Global objects #pragma region Global objects
AsyncWebServer webserver(80); AsyncWebServer webserver(80);
WebSocketsServer websocketserver(81); WebSocketsServer websocketserver(81);
Ticker ticker; Ticker ticker;
SoftwareSerial Serial3;
std::queue<RoverInstruction> InstrQueue;
#pragma endregion
// Global variables #pragma region Global variables
float battery_voltage = 4.0f; ControlStatus_t Status;
int distance_travelled = 0; float batteryVoltage;
float batteryLevel;
float batteryCycles;
int odometer;
int heading;
int xpos, ypos;
int signalStrength;
int lastExecutedCommand, lastCompletedCommand;
bool driveCommandComplete;
int bb_left, bb_right, bb_top, bb_bottom;
int bb_centre_x, bb_centre_y;
float chargeGoal;
#pragma endregion
void setup() void setup()
{ {
@ -39,10 +75,25 @@ void setup()
esp_log_level_set("wifi", ESP_LOG_WARN); // enable WARN logs from WiFi stack esp_log_level_set("wifi", ESP_LOG_WARN); // enable WARN logs from WiFi stack
esp_log_level_set("dhcpc", ESP_LOG_INFO); // enable INFO logs from DHCP client esp_log_level_set("dhcpc", ESP_LOG_INFO); // enable INFO logs from DHCP client
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 (Connected to Drive)
Serial2.begin(9600, SERIAL_8N1, 16, 17); // RX 9, TX 8 Serial2.begin(9600, SERIAL_8N1, RX2pin, TX2pin); // Set up hardware UART2 (Connected to Energy)
// Set up remaining communication ports here (Energy, Vision) Serial3.begin(9600, SWSERIAL_8N1, RX3pin, TX3pin); // Set up software UART3 (Connected to Vision)
// Set global variable startup values
Status = CS_IDLE;
batteryVoltage = 0;
batteryLevel = 0;
batteryCycles = 0;
odometer = 0;
heading = 0;
xpos = 0;
ypos = 0;
signalStrength = 0;
lastExecutedCommand = 0;
lastCompletedCommand = 0;
driveCommandComplete = 1;
chargeGoal = 0;
if (!SPIFFS.begin(true)) // Mount SPIFFS if (!SPIFFS.begin(true)) // Mount SPIFFS
{ {
@ -52,11 +103,11 @@ void setup()
Serial.println("SPIFFS mounted"); Serial.println("SPIFFS mounted");
WiFi.begin(WIFI_SSID, WIFI_PW); WiFi.begin(WIFI_SSID, WIFI_PW);
while (WiFi.status() != WL_CONNECTED) while (WiFi.status() != WL_CONNECTED) // Wait for ESP32 to connect to AP in "credentials.h"
{ {
delay(500); delay(500);
} }
while (!MDNS.begin("rover")) while (!MDNS.begin("rover")) // Set up mDNS cast at "rover.local/"
{ {
Serial.println("Error setting up mDNS, retrying in 5s"); Serial.println("Error setting up mDNS, retrying in 5s");
delay(5000); delay(5000);
@ -64,56 +115,118 @@ void setup()
Serial.println("mDNS set up, access Control Panel at 'rover.local/'"); Serial.println("mDNS set up, access Control Panel at 'rover.local/'");
webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request) webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request)
{ request->send(SPIFFS, "/index.html", "text/html"); }); { request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page
webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request) webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request)
{ request->send(SPIFFS, "/favicon.ico", "image/png"); }); { request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon
webserver.onNotFound(notFound); webserver.onNotFound(notFound); // Set up basic 404NotFound page
webserver.begin(); webserver.begin(); // Start Asynchronous Web Server
websocketserver.begin(); websocketserver.begin(); // Start Websocket Server
websocketserver.onEvent(webSocketEvent); websocketserver.onEvent(webSocketEvent); // Set up function call when event received from Command
ticker.attach(0.5, returnSensorData); ticker.attach(0.5, sendToCommand); // Set up recurring function to forward rover status to Command
} }
void loop() void loop()
{ {
printFPGAoutput();
String FPGAinput; // Forward serial monitor input to FPGA
if (Serial.available())
{
FPGAinput = String(Serial.readStringUntil('\n'));
Serial1.println(FPGAinput);
}
websocketserver.loop(); // Handle incoming client connections websocketserver.loop(); // Handle incoming client connections
} recvFromDrive(); // Update stats from Drive
recvFromEnergy(); // Update stats from Energy
void printFPGAoutput() recvFromVision(); // Update stats from Vision
{ // Print serial communication from FPGA to serial monitor switch (Status)
String FPGAoutput;
if (Serial1.available())
{ {
FPGAoutput = String(Serial1.readStringUntil('\n')); case CS_ERROR:
Serial.println(FPGAoutput);
}
}
void returnSensorData()
{
// Collect sensor data here?
distance_travelled++;
if (battery_voltage < 6)
{ {
battery_voltage += 0.2; Serial.println("Rover in error state, rebooting...");
exit(1);
} }
else break;
case CS_IDLE:
{ {
battery_voltage = 4; if (!InstrQueue.empty()) // If Rover idle and InstrQueue NOT empty: Do the next command in the queue
{
RoverInstruction *instr = &InstrQueue.front(); // Get next command
switch (instr->instr) // Determine command type
{
case INSTR_RESET: // Reset telemetry values (zeroing position/distance)
{
odometer = 0;
xpos = 0;
ypos = 0;
DynamicJsonDocument tdoc(128);
tdoc["rstD"] = 1;
serializeJson(tdoc, Serial1); // Send reset odometer signal to Drive
}
break;
case INSTR_STOP: // Emergency stop
{
Status = CS_ERROR;
while (1)
{
Serial.println("Emergency Stop should not get queued, hold and print");
delay(1000);
}
}
break;
case INSTR_MOVE: // Normal movement
{
Status = CS_MOVING; // Set moving state
driveCommandComplete = 0;
sendToDrive(*instr); // Forward to Drive handler
}
break;
case INSTR_CHARGE: // Normal charge
{
Status = CS_CHARGING; // Set charging state
chargeGoal = (float)instr->charge; // Set charging goal
sendToEnergy(1); // Forward to Energy handler
}
break;
default:
{
Serial.println("Unknown instruction type in queue, skipping...");
}
break;
}
lastExecutedCommand = instr->id; // Update tracker of last processed command
}
} }
String JSON_Data = String("{\"BTRY_VOLT\":") + battery_voltage + String(",\"ODO_DIST\":") + distance_travelled + "}"; break;
// Serial.println(JSON_Data); case CS_MOVING:
websocketserver.broadcastTXT(JSON_Data); {
if (driveCommandComplete) // If movement command complete:
{
Status = CS_IDLE; // Set rover state back to idle
lastCompletedCommand = lastExecutedCommand; // Update last completed command
}
else // If movement command NOT complete:
{ // Send (up to date) current heading to Drive
DynamicJsonDocument tdoc(128);
tdoc["rH"] = -1;
tdoc["cH"] = heading;
serializeJson(tdoc, Serial1);
}
}
break;
case CS_CHARGING:
{
if (batteryLevel >= chargeGoal) // Compare batteryLevel to chargeGoal
{
Status = CS_IDLE;
lastCompletedCommand = lastExecutedCommand; // Update last completed command
sendToEnergy(0); // Stop charging if goal reached
}
// Otherwise continue charging, no change
}
break;
default:
{
Serial.println("Unknown rover state, exiting...");
exit(1);
}
break;
}
delay(500);
} }
void notFound(AsyncWebServerRequest *request) void notFound(AsyncWebServerRequest *request)
@ -136,13 +249,13 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
Serial.printf("Client[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload); Serial.printf("Client[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
} }
break; break;
case WStype_TEXT: case WStype_TEXT: // MSG received from command panel
{ {
// Serial.printf("Client[%u] sent Text: %s\n", num, payload); Serial.printf("Client[%u] sent Text: %s\n", num, payload); // Echo received command to terminal
String command = String((char *)(payload)); String command = String((char *)(payload)); // Convert received command to string type
DynamicJsonDocument doc(200); //creating an instance of a DynamicJsonDocument allocating 200bytes on the heap. DynamicJsonDocument rdoc(200); // Create instance of DynamicJsonDocument on heap, 200 Bytes
DeserializationError error = deserializeJson(doc, command); // deserialize 'doc' and parse for parameters we expect to receive. DeserializationError error = deserializeJson(rdoc, command); // Convert command string to JSONDocument and capture any errors
if (error) if (error)
{ {
Serial.print("deserializeJson() failed: "); Serial.print("deserializeJson() failed: ");
@ -150,28 +263,60 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
return; return;
} }
int MVM_F_status = doc["MVM_F"]; RoverInstruction instr;
int MVM_L_status = doc["MVM_L"]; int mode = rdoc["mode"];
int MVM_R_status = doc["MVM_R"]; switch (mode)
int MVM_B_status = doc["MVM_B"]; {
case -1: // Add to queue, reset x/y/odometer (telemetry data)
{
Serial.println("Reset telemetry command received");
instr.id = rdoc["Cid"];
instr.instr = INSTR_RESET;
// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
Serial.println('<' + MVM_F_status + ',' + MVM_B_status + ',' + MVM_L_status + ',' + MVM_R_status + '>'); queueInstruction(instr); // Put reset command in InstrQueue
Serial.println("UpArrow=" + String(MVM_F_status));
if (MVM_F_status == 1){
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
tdoc["rH"] = 0;
tdoc["dist"] = 100;
tdoc["sp"] = 1;
serializeJson(tdoc, Serial2); // Build JSON and send on UART1
} }
if (MVM_B_status == 1){ break;
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be case 0: // Stop immediately, clear command cache
tdoc["rH"] = 0; {
tdoc["dist"] = -100; Serial.println("Emergency stop command received");
tdoc["sp"] = 1; // instr.instr = INSTR_STOP; // Not needed as Emergency Stop is not queued
serializeJson(tdoc, Serial2); // Build JSON and send on UART1 // Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
emergencyStop();
}
break;
case 1: // Normal movement command, added to end of command cache
{
Serial.println("Normal movement command received");
instr.id = rdoc["Cid"];
instr.instr = INSTR_MOVE;
instr.heading = rdoc["rH"];
instr.distance = rdoc["rD"];
instr.speed = rdoc["rS"];
// Ignore rdoc["rC"]
queueInstruction(instr); // Put movement command in InstrQueue
}
break;
case 2: // Normal charge command, results in no motion, added to end of command cache
{
Serial.println("Normal charge command received");
instr.id = rdoc["Cid"];
instr.instr = INSTR_CHARGE;
instr.charge = rdoc["rC"];
// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"]
queueInstruction(instr); // Put charge command in InstrQueue
}
break;
default:
{
// Default case, print and continue
Serial.println("Unknown Command type received, ignoring");
// Ignore rdoc["Cid"], rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
}
break;
} }
} }
break; break;
@ -185,5 +330,109 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
Serial.println(String("Websocket received invalid event type: ") + type + String(", exiting")); Serial.println(String("Websocket received invalid event type: ") + type + String(", exiting"));
exit(1); exit(1);
} }
break;
} }
} }
void queueInstruction(RoverInstruction instruction)
{
InstrQueue.push(instruction);
}
void sendToCommand()
{
DynamicJsonDocument tdoc(1024);
tdoc["st"] = Status;
tdoc["bV"] = batteryVoltage;
tdoc["bL"] = batteryLevel;
tdoc["bC"] = batteryCycles;
tdoc["tD"] = odometer;
tdoc["cH"] = heading;
tdoc["pos"][0] = xpos;
tdoc["pos"][1] = ypos;
tdoc["rssi"] = signalStrength;
tdoc["LCCid"] = lastCompletedCommand;
String JSON_Data;
serializeJson(tdoc, JSON_Data);
websocketserver.broadcastTXT(JSON_Data);
}
void sendToDrive(RoverInstruction instruction)
{
DynamicJsonDocument tdoc(1024);
tdoc["rH"] = instruction.heading;
tdoc["dist"] = instruction.distance;
tdoc["sp"] = instruction.speed;
tdoc["cH"] = heading;
serializeJson(tdoc, Serial1);
}
void recvFromDrive() // Update telemetry data and state info from Drive packet
{
if (Serial1.available()) // Check for input from UART1 (Connected to Drive)
{
DynamicJsonDocument rdoc(1024);
deserializeJson(rdoc, Serial1);
driveCommandComplete = rdoc["comp"];
odometer = rdoc["mm"];
xpos = rdoc["pos"][0];
ypos = rdoc["pos"][1];
}
}
void sendToEnergy(bool instruction)
{
DynamicJsonDocument tdoc(128);
tdoc["ch"] = instruction; // Start charging
serializeJson(tdoc, Serial2);
}
void recvFromEnergy() // Update telemetry data and state info from Energy packet
{
if (Serial2.available()) // Check for input from UART2 (Connected to Energy)
{
DynamicJsonDocument rdoc(1024);
deserializeJson(rdoc, Serial2);
batteryLevel = rdoc["soc"];
batteryVoltage = rdoc["mV"];
batteryCycles = rdoc["cyc"];
}
}
void sendToVision()
{
Serial3.print("R"); // Request new data from Vision
}
void recvFromVision() // Update bounding box and obstacle detection data from Vision packet
{
if (Serial3.available()) // Check for input from UART3 (Connected to Vision)
{
DynamicJsonDocument rdoc(1024);
deserializeJson(rdoc, Serial3);
bb_left = rdoc["bb"][0];
bb_right = rdoc["bb"][1];
bb_top = rdoc["bb"][2];
bb_bottom = rdoc["bb"][3];
bb_centre_x = rdoc["cen"][0];
bb_centre_y = rdoc["cen"][1];
heading = rdoc["cH"];
}
}
void emergencyStop()
{
DynamicJsonDocument tdoc(1024);
tdoc["rH"] = heading;
tdoc["dist"] = -1;
tdoc["sp"] = -1;
tdoc["cH"] = heading;
serializeJson(tdoc, Serial1); // Send stop signals to Drive
sendToEnergy(0); // Send stop signal to Energy
while (InstrQueue.size())
{
InstrQueue.pop(); // Clear Instruction Queue
}
Status = CS_IDLE; // Reset rover to idle state
Serial.println("Instruction Queue cleared");
}

448
Vision/DE10_LITE_D8M_VIP_16/software/D8M_Camera_Test/main.c
View file

@ -1,5 +1,3 @@
#include <stdio.h> #include <stdio.h>
#include "I2C_core.h" #include "I2C_core.h"
#include "terasic_includes.h" #include "terasic_includes.h"
@ -12,7 +10,7 @@
#include <unistd.h> #include <unistd.h>
//EEE_IMGPROC defines //EEE_IMGPROC defines
#define EEE_IMGPROC_MSG_START ('R'<<16 | 'B'<<8 | 'B') #define EEE_IMGPROC_MSG_START ('R' << 16 | 'B' << 8 | 'B')
//offsets //offsets
#define EEE_IMGPROC_STATUS 0 #define EEE_IMGPROC_STATUS 0
@ -26,286 +24,242 @@
#define GAIN_STEP 0x040 #define GAIN_STEP 0x040
#define DEFAULT_LEVEL 3 #define DEFAULT_LEVEL 3
#define MIPI_REG_PHYClkCtl 0x0056 #define MIPI_REG_PHYClkCtl 0x0056
#define MIPI_REG_PHYData0Ctl 0x0058 #define MIPI_REG_PHYData0Ctl 0x0058
#define MIPI_REG_PHYData1Ctl 0x005A #define MIPI_REG_PHYData1Ctl 0x005A
#define MIPI_REG_PHYData2Ctl 0x005C #define MIPI_REG_PHYData2Ctl 0x005C
#define MIPI_REG_PHYData3Ctl 0x005E #define MIPI_REG_PHYData3Ctl 0x005E
#define MIPI_REG_PHYTimDly 0x0060 #define MIPI_REG_PHYTimDly 0x0060
#define MIPI_REG_PHYSta 0x0062 #define MIPI_REG_PHYSta 0x0062
#define MIPI_REG_CSIStatus 0x0064 #define MIPI_REG_CSIStatus 0x0064
#define MIPI_REG_CSIErrEn 0x0066 #define MIPI_REG_CSIErrEn 0x0066
#define MIPI_REG_MDLSynErr 0x0068 #define MIPI_REG_MDLSynErr 0x0068
#define MIPI_REG_FrmErrCnt 0x0080 #define MIPI_REG_FrmErrCnt 0x0080
#define MIPI_REG_MDLErrCnt 0x0090 #define MIPI_REG_MDLErrCnt 0x0090
void mipi_clear_error(void){ void mipi_clear_error(void)
MipiBridgeRegWrite(MIPI_REG_CSIStatus,0x01FF); // clear error {
MipiBridgeRegWrite(MIPI_REG_MDLSynErr,0x0000); // clear error MipiBridgeRegWrite(MIPI_REG_CSIStatus, 0x01FF); // clear error
MipiBridgeRegWrite(MIPI_REG_FrmErrCnt,0x0000); // clear error MipiBridgeRegWrite(MIPI_REG_MDLSynErr, 0x0000); // clear error
MipiBridgeRegWrite(MIPI_REG_MDLErrCnt, 0x0000); // clear error MipiBridgeRegWrite(MIPI_REG_FrmErrCnt, 0x0000); // clear error
MipiBridgeRegWrite(MIPI_REG_MDLErrCnt, 0x0000); // clear error
MipiBridgeRegWrite(0x0082,0x00); MipiBridgeRegWrite(0x0082, 0x00);
MipiBridgeRegWrite(0x0084,0x00); MipiBridgeRegWrite(0x0084, 0x00);
MipiBridgeRegWrite(0x0086,0x00); MipiBridgeRegWrite(0x0086, 0x00);
MipiBridgeRegWrite(0x0088,0x00); MipiBridgeRegWrite(0x0088, 0x00);
MipiBridgeRegWrite(0x008A,0x00); MipiBridgeRegWrite(0x008A, 0x00);
MipiBridgeRegWrite(0x008C,0x00); MipiBridgeRegWrite(0x008C, 0x00);
MipiBridgeRegWrite(0x008E,0x00); MipiBridgeRegWrite(0x008E, 0x00);
MipiBridgeRegWrite(0x0090,0x00); MipiBridgeRegWrite(0x0090, 0x00);
} }
void mipi_show_error_info(void){ void mipi_show_error_info(void)
{
alt_u16 PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt; alt_u16 PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt;
PHY_status = MipiBridgeRegRead(MIPI_REG_PHYSta); PHY_status = MipiBridgeRegRead(MIPI_REG_PHYSta);
SCI_status = MipiBridgeRegRead(MIPI_REG_CSIStatus); SCI_status = MipiBridgeRegRead(MIPI_REG_CSIStatus);
MDLSynErr = MipiBridgeRegRead(MIPI_REG_MDLSynErr); MDLSynErr = MipiBridgeRegRead(MIPI_REG_MDLSynErr);
FrmErrCnt = MipiBridgeRegRead(MIPI_REG_FrmErrCnt); FrmErrCnt = MipiBridgeRegRead(MIPI_REG_FrmErrCnt);
MDLErrCnt = MipiBridgeRegRead(MIPI_REG_MDLErrCnt); MDLErrCnt = MipiBridgeRegRead(MIPI_REG_MDLErrCnt);
printf("PHY_status=%xh, CSI_status=%xh, MDLSynErr=%xh, FrmErrCnt=%xh, MDLErrCnt=%xh\r\n", PHY_status, SCI_status, MDLSynErr,FrmErrCnt, MDLErrCnt); printf("PHY_status=%xh, CSI_status=%xh, MDLSynErr=%xh, FrmErrCnt=%xh, MDLErrCnt=%xh\r\n", PHY_status, SCI_status, MDLSynErr, FrmErrCnt, MDLErrCnt);
} }
void mipi_show_error_info_more(void){ void mipi_show_error_info_more(void)
printf("FrmErrCnt = %d\n",MipiBridgeRegRead(0x0080)); {
printf("CRCErrCnt = %d\n",MipiBridgeRegRead(0x0082)); printf("FrmErrCnt = %d\n", MipiBridgeRegRead(0x0080));
printf("CorErrCnt = %d\n",MipiBridgeRegRead(0x0084)); printf("CRCErrCnt = %d\n", MipiBridgeRegRead(0x0082));
printf("HdrErrCnt = %d\n",MipiBridgeRegRead(0x0086)); printf("CorErrCnt = %d\n", MipiBridgeRegRead(0x0084));
printf("EIDErrCnt = %d\n",MipiBridgeRegRead(0x0088)); printf("HdrErrCnt = %d\n", MipiBridgeRegRead(0x0086));
printf("CtlErrCnt = %d\n",MipiBridgeRegRead(0x008A)); printf("EIDErrCnt = %d\n", MipiBridgeRegRead(0x0088));
printf("SoTErrCnt = %d\n",MipiBridgeRegRead(0x008C)); printf("CtlErrCnt = %d\n", MipiBridgeRegRead(0x008A));
printf("SynErrCnt = %d\n",MipiBridgeRegRead(0x008E)); printf("SoTErrCnt = %d\n", MipiBridgeRegRead(0x008C));
printf("MDLErrCnt = %d\n",MipiBridgeRegRead(0x0090)); printf("SynErrCnt = %d\n", MipiBridgeRegRead(0x008E));
printf("FIFOSTATUS = %d\n",MipiBridgeRegRead(0x00F8)); printf("MDLErrCnt = %d\n", MipiBridgeRegRead(0x0090));
printf("DataType = 0x%04x\n",MipiBridgeRegRead(0x006A)); printf("FIFOSTATUS = %d\n", MipiBridgeRegRead(0x00F8));
printf("CSIPktLen = %d\n",MipiBridgeRegRead(0x006E)); printf("DataType = 0x%04x\n", MipiBridgeRegRead(0x006A));
printf("CSIPktLen = %d\n", MipiBridgeRegRead(0x006E));
} }
bool MIPI_Init(void)
{
bool bSuccess;
bSuccess = oc_i2c_init_ex(I2C_OPENCORES_MIPI_BASE, 50 * 1000 * 1000, 400 * 1000); //I2C: 400K
if (!bSuccess)
{
printf("failed to init MIPI- Bridge i2c\r\n");
}
bool MIPI_Init(void){ usleep(50 * 1000);
bool bSuccess;
bSuccess = oc_i2c_init_ex(I2C_OPENCORES_MIPI_BASE, 50*1000*1000,400*1000); //I2C: 400K
if (!bSuccess)
printf("failed to init MIPI- Bridge i2c\r\n");
usleep(50*1000);
MipiBridgeInit(); MipiBridgeInit();
usleep(500*1000); usleep(500 * 1000);
// bSuccess = oc_i2c_init_ex(I2C_OPENCORES_CAMERA_BASE, 50*1000*1000,400*1000); //I2C: 400K
// if (!bSuccess)
// printf("failed to init MIPI- Camera i2c\r\n");
MipiCameraInit(); MipiCameraInit();
MIPI_BIN_LEVEL(DEFAULT_LEVEL); MIPI_BIN_LEVEL(DEFAULT_LEVEL);
// OV8865_FOCUS_Move_to(340);
// oc_i2c_uninit(I2C_OPENCORES_CAMERA_BASE); // Release I2C bus , due to two I2C master shared! usleep(1000);
return bSuccess;
usleep(1000);
// oc_i2c_uninit(I2C_OPENCORES_MIPI_BASE);
return bSuccess;
} }
int main() int main()
{ {
fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK); printf("DE10-LITE D8M VGA Demo\n");
printf("Imperial College EEE2 Project version\n");
printf("DE10-LITE D8M VGA Demo\n"); IOWR(MIPI_PWDN_N_BASE, 0x00, 0x00);
printf("Imperial College EEE2 Project version\n"); IOWR(MIPI_RESET_N_BASE, 0x00, 0x00);
IOWR(MIPI_PWDN_N_BASE, 0x00, 0x00);
IOWR(MIPI_RESET_N_BASE, 0x00, 0x00);
usleep(2000);
IOWR(MIPI_PWDN_N_BASE, 0x00, 0xFF);
usleep(2000);
IOWR(MIPI_RESET_N_BASE, 0x00, 0xFF);
printf("Image Processor ID: %x\n",IORD(0x42000,EEE_IMGPROC_ID));
//printf("Image Processor ID: %x\n",IORD(EEE_IMGPROC_0_BASE,EEE_IMGPROC_ID)); //Don't know why this doesn't work - definition is in system.h in BSP
usleep(2000);
// MIPI Init
if (!MIPI_Init()){
printf("MIPI_Init Init failed!\r\n");
}else{
printf("MIPI_Init Init successfully!\r\n");
}
// while(1){
mipi_clear_error();
usleep(50*1000);
mipi_clear_error();
usleep(1000*1000);
mipi_show_error_info();
// mipi_show_error_info_more();
printf("\n");
// }
#if 0 // focus sweep
printf("\nFocus sweep\n");
alt_u16 ii= 350;
alt_u8 dir = 0;
while(1){
if(ii< 50) dir = 1;
else if (ii> 1000) dir =0;
if(dir) ii += 20;
else ii -= 20;
printf("%d\n",ii);
OV8865_FOCUS_Move_to(ii);
usleep(50*1000);
}
#endif
usleep(2000);
IOWR(MIPI_PWDN_N_BASE, 0x00, 0xFF);
usleep(2000);
IOWR(MIPI_RESET_N_BASE, 0x00, 0xFF);
printf("Image Processor ID: %x\n", IORD(0x42000, EEE_IMGPROC_ID));
//printf("Image Processor ID: %x\n",IORD(EEE_IMGPROC_0_BASE,EEE_IMGPROC_ID)); //Don't know why this doesn't work - definition is in system.h in BSP
usleep(2000);
// MIPI Init
if (!MIPI_Init())
{
printf("MIPI_Init Init failed!\r\n");
}
else
{
printf("MIPI_Init Init successfully!\r\n");
}
// while(1){
mipi_clear_error();
usleep(50 * 1000);
mipi_clear_error();
usleep(1000 * 1000);
mipi_show_error_info();
// mipi_show_error_info_more();
printf("\n");
// }
////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////
alt_u16 bin_level = DEFAULT_LEVEL; alt_u16 bin_level = DEFAULT_LEVEL;
alt_u8 manual_focus_step = 10; alt_u8 manual_focus_step = 10;
alt_u16 current_focus = 300; alt_u16 current_focus = 300;
int boundingBoxColour = 0; int boundingBoxColour = 0;
alt_u32 exposureTime = EXPOSURE_INIT; alt_u32 exposureTime = EXPOSURE_INIT;
alt_u16 gain = GAIN_INIT; alt_u16 gain = GAIN_INIT;
OV8865SetExposure(exposureTime); OV8865SetExposure(exposureTime);
OV8865SetGain(gain); OV8865SetGain(gain);
Focus_Init(); Focus_Init();
FILE* ser = fopen("/dev/uart_0", "rb+"); FILE *ser = fopen("/dev/uart_0", "rb+");
if(ser){ fcntl(ser, F_SETFL, O_NONBLOCK);
printf("Opened UART\n"); if (ser)
} else { {
printf("Failed to open UART\n"); printf("Opened UART\n");
while (1); }
else
{
printf("Failed to open UART\n");
while (1)
{
}
}
while (1)
{
// touch KEY0 to trigger Auto focus
if ((IORD(KEY_BASE, 0) & 0x03) == 0x02)
{
current_focus = Focus_Window(320, 240);
}
// touch KEY1 to ZOOM
if ((IORD(KEY_BASE, 0) & 0x03) == 0x01)
{
if (bin_level == 3)
bin_level = 1;
else
bin_level++;
printf("set bin level to %d\n", bin_level);
MIPI_BIN_LEVEL(bin_level);
usleep(500000);
} }
while(1){ //Read messages from the image processor and print them on the terminal
while ((IORD(0x42000, EEE_IMGPROC_STATUS) >> 8) & 0xff)
{ //Find out if there are words to read
int word = IORD(0x42000, EEE_IMGPROC_MSG); //Get next word from message buffer
if (fwrite(&word, 4, 1, ser) != 1)
printf("Error writing to UART");
if (word == EEE_IMGPROC_MSG_START) //Newline on message identifier
printf("\n");
printf("%08x ", word);
}
// touch KEY0 to trigger Auto focus //Update the bounding box colour
if((IORD(KEY_BASE,0)&0x03) == 0x02){ boundingBoxColour = ((boundingBoxColour + 1) & 0xff);
IOWR(0x42000, EEE_IMGPROC_BBCOL, (boundingBoxColour << 8) | (0xff - boundingBoxColour));
current_focus = Focus_Window(320,240); //Process input commands
} int in = getchar();
// touch KEY1 to ZOOM switch (in)
if((IORD(KEY_BASE,0)&0x03) == 0x01){ {
if(bin_level == 3 )bin_level = 1; case 'e':
else bin_level ++; {
printf("set bin level to %d\n",bin_level); exposureTime += EXPOSURE_STEP;
MIPI_BIN_LEVEL(bin_level); OV8865SetExposure(exposureTime);
usleep(500000); printf("\nExposure = %x ", exposureTime);
break;
}
case 'd':
{
exposureTime -= EXPOSURE_STEP;
OV8865SetExposure(exposureTime);
printf("\nExposure = %x ", exposureTime);
break;
}
case 't':
{
gain += GAIN_STEP;
OV8865SetGain(gain);
printf("\nGain = %x ", gain);
break;
}
case 'g':
{
gain -= GAIN_STEP;
OV8865SetGain(gain);
printf("\nGain = %x ", gain);
break;
}
case 'r':
{
current_focus += manual_focus_step;
if (current_focus > 1023)
current_focus = 1023;
OV8865_FOCUS_Move_to(current_focus);
printf("\nFocus = %x ", current_focus);
break;
}
case 'f':
{
if (current_focus > manual_focus_step)
current_focus -= manual_focus_step;
OV8865_FOCUS_Move_to(current_focus);
printf("\nFocus = %x ", current_focus);
break;
}
}
} //Main loop delay
usleep(10000);
};
#if 0 return 0;
if((IORD(KEY_BASE,0)&0x0F) == 0x0E){
current_focus = Focus_Window(320,240);
}
// touch KEY1 to trigger Manual focus - step
if((IORD(KEY_BASE,0)&0x0F) == 0x0D){
if(current_focus > manual_focus_step) current_focus -= manual_focus_step;
else current_focus = 0;
OV8865_FOCUS_Move_to(current_focus);
}
// touch KEY2 to trigger Manual focus + step
if((IORD(KEY_BASE,0)&0x0F) == 0x0B){
current_focus += manual_focus_step;
if(current_focus >1023) current_focus = 1023;
OV8865_FOCUS_Move_to(current_focus);
}
// touch KEY3 to ZOOM
if((IORD(KEY_BASE,0)&0x0F) == 0x07){
if(bin_level == 3 )bin_level = 1;
else bin_level ++;
printf("set bin level to %d\n",bin_level);
MIPI_BIN_LEVEL(bin_level);
usleep(500000);
}
#endif
//Read messages from the image processor and print them on the terminal
while ((IORD(0x42000,EEE_IMGPROC_STATUS)>>8) & 0xff) { //Find out if there are words to read
int word = IORD(0x42000,EEE_IMGPROC_MSG); //Get next word from message buffer
if (fwrite(&word, 4, 1, ser) != 1)
printf("Error writing to UART");
if (word == EEE_IMGPROC_MSG_START) //Newline on message identifier
printf("\n");
printf("%08x ",word);
}
//Update the bounding box colour
boundingBoxColour = ((boundingBoxColour + 1) & 0xff);
IOWR(0x42000, EEE_IMGPROC_BBCOL, (boundingBoxColour << 8) | (0xff - boundingBoxColour));
//Process input commands
int in = getchar();
switch (in) {
case 'e': {
exposureTime += EXPOSURE_STEP;
OV8865SetExposure(exposureTime);
printf("\nExposure = %x ", exposureTime);
break;}
case 'd': {
exposureTime -= EXPOSURE_STEP;
OV8865SetExposure(exposureTime);
printf("\nExposure = %x ", exposureTime);
break;}
case 't': {
gain += GAIN_STEP;
OV8865SetGain(gain);
printf("\nGain = %x ", gain);
break;}
case 'g': {
gain -= GAIN_STEP;
OV8865SetGain(gain);
printf("\nGain = %x ", gain);
break;}
case 'r': {
current_focus += manual_focus_step;
if(current_focus >1023) current_focus = 1023;
OV8865_FOCUS_Move_to(current_focus);
printf("\nFocus = %x ",current_focus);
break;}
case 'f': {
if(current_focus > manual_focus_step) current_focus -= manual_focus_step;
OV8865_FOCUS_Move_to(current_focus);
printf("\nFocus = %x ",current_focus);
break;}
}
//Main loop delay
usleep(10000);
};
return 0;
} }