mirror of
https://github.com/supleed2/ELEC50003-P1-CW.git
synced 2024-12-22 21:45:49 +00:00
Integrated Command and Drive
This commit is contained in:
Mee2001
parent
eac44e1d3d
commit
f40452c255
|
|
@ -13,7 +13,7 @@ platform = espressif32
|
||||||
board = esp32dev
|
board = esp32dev
|
||||||
framework = arduino
|
framework = arduino
|
||||||
monitor_speed = 115200
|
monitor_speed = 115200
|
||||||
upload_port = COM[3]
|
upload_port = COM13
|
||||||
monitor_filters =
|
monitor_filters =
|
||||||
send_on_enter
|
send_on_enter
|
||||||
esp32_exception_decoder
|
esp32_exception_decoder
|
||||||
|
|
|
||||||
|
|
@ -19,11 +19,11 @@ void loop()
|
||||||
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"];
|
||||||
float speed = rdoc["sp"];
|
float spd = rdoc["sp"];
|
||||||
int currentHeading = rdoc["cH"];
|
int currentHeading = rdoc["cH"];
|
||||||
|
|
||||||
bool commandComplete = 0;
|
bool commandComplete = 0;
|
||||||
float powerUsage_mW = 0.0;
|
float powerUsage_mWh = 0.0;
|
||||||
int distTravelled_mm = 0;
|
int distTravelled_mm = 0;
|
||||||
int current_x = 0;
|
int current_x = 0;
|
||||||
int current_y = 0;
|
int current_y = 0;
|
||||||
|
|
@ -32,7 +32,7 @@ void loop()
|
||||||
|
|
||||||
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
|
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
|
||||||
tdoc["comp"] = commandComplete; // If 0: command in progress, current heading requested
|
tdoc["comp"] = commandComplete; // If 0: command in progress, current heading requested
|
||||||
tdoc["mW"] = powerUsage_mW;
|
tdoc["mWh"] = powerUsage_mWh;
|
||||||
tdoc["mm"] = distTravelled_mm;
|
tdoc["mm"] = distTravelled_mm;
|
||||||
tdoc["pos"][0] = current_x;
|
tdoc["pos"][0] = current_x;
|
||||||
tdoc["pos"][1] = current_y;
|
tdoc["pos"][1] = current_y;
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
#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 <AsyncTCP.h>
|
#include <AsyncTCP.h>
|
||||||
#include <ESPAsyncWebServer.h>
|
#include <ESPAsyncWebServer.h>
|
||||||
#include <ESPmDNS.h>
|
#include <ESPmDNS.h>
|
||||||
|
|
@ -41,7 +41,8 @@ void setup()
|
||||||
|
|
||||||
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
|
||||||
// Set up remaining communication ports here (Energy, Drive, Vision)
|
Serial2.begin(9600, SERIAL_8N1, 16, 17); // RX 9, TX 8
|
||||||
|
// Set up remaining communication ports here (Energy, Vision)
|
||||||
|
|
||||||
if (!SPIFFS.begin(true)) // Mount SPIFFS
|
if (!SPIFFS.begin(true)) // Mount SPIFFS
|
||||||
{
|
{
|
||||||
|
|
@ -111,7 +112,7 @@ void returnSensorData()
|
||||||
battery_voltage = 4;
|
battery_voltage = 4;
|
||||||
}
|
}
|
||||||
String JSON_Data = String("{\"BTRY_VOLT\":") + battery_voltage + String(",\"ODO_DIST\":") + distance_travelled + "}";
|
String JSON_Data = String("{\"BTRY_VOLT\":") + battery_voltage + String(",\"ODO_DIST\":") + distance_travelled + "}";
|
||||||
Serial.println(JSON_Data);
|
// Serial.println(JSON_Data);
|
||||||
websocketserver.broadcastTXT(JSON_Data);
|
websocketserver.broadcastTXT(JSON_Data);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -137,7 +138,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
|
||||||
break;
|
break;
|
||||||
case WStype_TEXT:
|
case WStype_TEXT:
|
||||||
{
|
{
|
||||||
Serial.printf("Client[%u] sent Text: %s\n", num, payload);
|
// Serial.printf("Client[%u] sent Text: %s\n", num, payload);
|
||||||
String command = String((char *)(payload));
|
String command = String((char *)(payload));
|
||||||
|
|
||||||
DynamicJsonDocument doc(200); //creating an instance of a DynamicJsonDocument allocating 200bytes on the heap.
|
DynamicJsonDocument doc(200); //creating an instance of a DynamicJsonDocument allocating 200bytes on the heap.
|
||||||
|
|
@ -155,6 +156,23 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
|
||||||
int MVM_B_status = doc["MVM_B"];
|
int MVM_B_status = doc["MVM_B"];
|
||||||
|
|
||||||
Serial.println('<' + MVM_F_status + ',' + MVM_B_status + ',' + MVM_L_status + ',' + MVM_R_status + '>');
|
Serial.println('<' + MVM_F_status + ',' + MVM_B_status + ',' + MVM_L_status + ',' + MVM_R_status + '>');
|
||||||
|
|
||||||
|
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){
|
||||||
|
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
|
||||||
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case WStype_PONG:
|
case WStype_PONG:
|
||||||
|
|
|
||||||
5
Drive/.gitignore
vendored
Normal file
5
Drive/.gitignore
vendored
Normal file
|
|
@ -0,0 +1,5 @@
|
||||||
|
.pio
|
||||||
|
.vscode/.browse.c_cpp.db*
|
||||||
|
.vscode/c_cpp_properties.json
|
||||||
|
.vscode/launch.json
|
||||||
|
.vscode/ipch
|
||||||
7
Drive/.vscode/extensions.json
vendored
Normal file
7
Drive/.vscode/extensions.json
vendored
Normal file
|
|
@ -0,0 +1,7 @@
|
||||||
|
{
|
||||||
|
// See http://go.microsoft.com/fwlink/?LinkId=827846
|
||||||
|
// for the documentation about the extensions.json format
|
||||||
|
"recommendations": [
|
||||||
|
"platformio.platformio-ide"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
|
@ -1,795 +0,0 @@
|
||||||
#include <Arduino.h>
|
|
||||||
#include <ArduinoJson.h>
|
|
||||||
#include <string>
|
|
||||||
#include <Wire.h>
|
|
||||||
#include <INA219_WE.h>
|
|
||||||
#include "SPI.h"
|
|
||||||
#include <SoftwareSerial.h>
|
|
||||||
SoftwareSerial Serial4 = SoftwareSerial(6, 10);
|
|
||||||
|
|
||||||
#define RXpin 0 // Define your RX pin here
|
|
||||||
#define TXpin 0 // Define your TX pin here
|
|
||||||
|
|
||||||
bool debug = true;
|
|
||||||
|
|
||||||
//TO IMPLEMENT
|
|
||||||
//DONE 2 way serial
|
|
||||||
//DONE F<>,B<>,S,L<>,R<>,p<0--1023>
|
|
||||||
//DONE Obtain current and power usage, get voltage from analog pin
|
|
||||||
//request angle facing
|
|
||||||
//DONE speed control 0-1
|
|
||||||
//speed calibration, 0 stop and max speed to match
|
|
||||||
//distance travveled and x and y at request
|
|
||||||
|
|
||||||
|
|
||||||
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
|
|
||||||
INA219_WE ina219; // this is the instantiation of the library for the current sensor
|
|
||||||
|
|
||||||
float open_loop, closed_loop; // Duty Cycles
|
|
||||||
float vpd, vb, vref, iL, dutyref, current_mA; // Measurement Variables
|
|
||||||
unsigned int sensorValue0, sensorValue1, sensorValue2, sensorValue3; // ADC sample values declaration
|
|
||||||
float ev = 0, cv = 0, ei = 0, oc = 0; //internal signals
|
|
||||||
float Ts = 0.0008; //1.25 kHz control frequency. It's better to design the control period as integral multiple of switching period.
|
|
||||||
float kpv = 0.05024, kiv = 15.78, kdv = 0; // voltage pid.
|
|
||||||
float u0v, u1v, delta_uv, e0v, e1v, e2v; // u->output; e->error; 0->this time; 1->last time; 2->last last time
|
|
||||||
float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid.
|
|
||||||
float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller
|
|
||||||
float uv_max = 4, uv_min = 0; //anti-windup limitation
|
|
||||||
float ui_max = 1, ui_min = 0; //anti-windup limitation
|
|
||||||
float current_limit = 1.0;
|
|
||||||
boolean Boost_mode = 0;
|
|
||||||
boolean CL_mode = 0;
|
|
||||||
|
|
||||||
|
|
||||||
unsigned int loopTrigger;
|
|
||||||
unsigned int com_count = 0; // a variables to count the interrupts. Used for program debugging.
|
|
||||||
|
|
||||||
|
|
||||||
//************************** Motor Constants **************************//
|
|
||||||
unsigned long previousMillis = 0; //initializing time counter
|
|
||||||
const long f_i = 10000; //time to move in forward direction, please calculate the precision and conversion factor
|
|
||||||
const long r_i = 20000; //time to rotate clockwise
|
|
||||||
const long b_i = 30000; //time to move backwards
|
|
||||||
const long l_i = 40000; //time to move anticlockwise
|
|
||||||
const long s_i = 50000;
|
|
||||||
int DIRRstate = LOW; //initializing direction states
|
|
||||||
int DIRLstate = HIGH;
|
|
||||||
|
|
||||||
int DIRL = 20; //defining left direction pin
|
|
||||||
int DIRR = 21; //defining right direction pin
|
|
||||||
|
|
||||||
int pwmr = 5; //pin to control right wheel speed using pwm
|
|
||||||
int pwml = 9; //pin to control left wheel speed using pwm
|
|
||||||
//*******************************************************************//
|
|
||||||
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
|
|
||||||
|
|
||||||
//-------------------------------------------------------OPTICAL SENSOR CODE START------------------------------------------------------//
|
|
||||||
#define PIN_SS 10
|
|
||||||
#define PIN_MISO 12
|
|
||||||
#define PIN_MOSI 11
|
|
||||||
#define PIN_SCK 13
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#define PIN_MOUSECAM_RESET 8
|
|
||||||
#define PIN_MOUSECAM_CS 7
|
|
||||||
|
|
||||||
#define ADNS3080_PIXELS_X 30
|
|
||||||
#define ADNS3080_PIXELS_Y 30
|
|
||||||
|
|
||||||
#define ADNS3080_PRODUCT_ID 0x00
|
|
||||||
#define ADNS3080_REVISION_ID 0x01
|
|
||||||
#define ADNS3080_MOTION 0x02
|
|
||||||
#define ADNS3080_DELTA_X 0x03
|
|
||||||
#define ADNS3080_DELTA_Y 0x04
|
|
||||||
#define ADNS3080_SQUAL 0x05
|
|
||||||
#define ADNS3080_PIXEL_SUM 0x06
|
|
||||||
#define ADNS3080_MAXIMUM_PIXEL 0x07
|
|
||||||
#define ADNS3080_CONFIGURATION_BITS 0x0a
|
|
||||||
#define ADNS3080_EXTENDED_CONFIG 0x0b
|
|
||||||
#define ADNS3080_DATA_OUT_LOWER 0x0c
|
|
||||||
#define ADNS3080_DATA_OUT_UPPER 0x0d
|
|
||||||
#define ADNS3080_SHUTTER_LOWER 0x0e
|
|
||||||
#define ADNS3080_SHUTTER_UPPER 0x0f
|
|
||||||
#define ADNS3080_FRAME_PERIOD_LOWER 0x10
|
|
||||||
#define ADNS3080_FRAME_PERIOD_UPPER 0x11
|
|
||||||
#define ADNS3080_MOTION_CLEAR 0x12
|
|
||||||
#define ADNS3080_FRAME_CAPTURE 0x13
|
|
||||||
#define ADNS3080_SROM_ENABLE 0x14
|
|
||||||
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19
|
|
||||||
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a
|
|
||||||
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b
|
|
||||||
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c
|
|
||||||
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e
|
|
||||||
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e
|
|
||||||
#define ADNS3080_SROM_ID 0x1f
|
|
||||||
#define ADNS3080_OBSERVATION 0x3d
|
|
||||||
#define ADNS3080_INVERSE_PRODUCT_ID 0x3f
|
|
||||||
#define ADNS3080_PIXEL_BURST 0x40
|
|
||||||
#define ADNS3080_MOTION_BURST 0x50
|
|
||||||
#define ADNS3080_SROM_LOAD 0x60
|
|
||||||
|
|
||||||
#define ADNS3080_PRODUCT_ID_VAL 0x17
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int total_x = 0;
|
|
||||||
|
|
||||||
int total_y = 0;
|
|
||||||
|
|
||||||
|
|
||||||
int total_x1 = 0;
|
|
||||||
|
|
||||||
int total_y1 = 0;
|
|
||||||
|
|
||||||
|
|
||||||
int x=0;
|
|
||||||
int y=0;
|
|
||||||
|
|
||||||
int a=0;
|
|
||||||
int b=0;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int distance_x=0;
|
|
||||||
int distance_y=0;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
volatile byte movementflag=0;
|
|
||||||
volatile int xydat[2];
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int convTwosComp(int b){
|
|
||||||
//Convert from 2's complement
|
|
||||||
if(b & 0x80){
|
|
||||||
b = -1 * ((b ^ 0xff) + 1);
|
|
||||||
}
|
|
||||||
return b;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int tdistance = 0;
|
|
||||||
|
|
||||||
|
|
||||||
void mousecam_reset()
|
|
||||||
{
|
|
||||||
digitalWrite(PIN_MOUSECAM_RESET,HIGH);
|
|
||||||
delay(1); // reset pulse >10us
|
|
||||||
digitalWrite(PIN_MOUSECAM_RESET,LOW);
|
|
||||||
delay(35); // 35ms from reset to functional
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int mousecam_init()
|
|
||||||
{
|
|
||||||
pinMode(PIN_MOUSECAM_RESET,OUTPUT);
|
|
||||||
pinMode(PIN_MOUSECAM_CS,OUTPUT);
|
|
||||||
|
|
||||||
digitalWrite(PIN_MOUSECAM_CS,HIGH);
|
|
||||||
|
|
||||||
mousecam_reset();
|
|
||||||
|
|
||||||
int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID);
|
|
||||||
if(pid != ADNS3080_PRODUCT_ID_VAL)
|
|
||||||
return -1;
|
|
||||||
|
|
||||||
// turn on sensitive mode
|
|
||||||
mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
void mousecam_write_reg(int reg, int val)
|
|
||||||
{
|
|
||||||
digitalWrite(PIN_MOUSECAM_CS, LOW);
|
|
||||||
SPI.transfer(reg | 0x80);
|
|
||||||
SPI.transfer(val);
|
|
||||||
digitalWrite(PIN_MOUSECAM_CS,HIGH);
|
|
||||||
delayMicroseconds(50);
|
|
||||||
}
|
|
||||||
|
|
||||||
int mousecam_read_reg(int reg)
|
|
||||||
{
|
|
||||||
digitalWrite(PIN_MOUSECAM_CS, LOW);
|
|
||||||
SPI.transfer(reg);
|
|
||||||
delayMicroseconds(75);
|
|
||||||
int ret = SPI.transfer(0xff);
|
|
||||||
digitalWrite(PIN_MOUSECAM_CS,HIGH);
|
|
||||||
delayMicroseconds(1);
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
struct MD
|
|
||||||
{
|
|
||||||
byte motion;
|
|
||||||
char dx, dy;
|
|
||||||
byte squal;
|
|
||||||
word shutter;
|
|
||||||
byte max_pix;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
void mousecam_read_motion(struct MD *p)
|
|
||||||
{
|
|
||||||
digitalWrite(PIN_MOUSECAM_CS, LOW);
|
|
||||||
SPI.transfer(ADNS3080_MOTION_BURST);
|
|
||||||
delayMicroseconds(75);
|
|
||||||
p->motion = SPI.transfer(0xff);
|
|
||||||
p->dx = SPI.transfer(0xff);
|
|
||||||
p->dy = SPI.transfer(0xff);
|
|
||||||
p->squal = SPI.transfer(0xff);
|
|
||||||
p->shutter = SPI.transfer(0xff)<<8;
|
|
||||||
p->shutter |= SPI.transfer(0xff);
|
|
||||||
p->max_pix = SPI.transfer(0xff);
|
|
||||||
digitalWrite(PIN_MOUSECAM_CS,HIGH);
|
|
||||||
delayMicroseconds(5);
|
|
||||||
}
|
|
||||||
|
|
||||||
// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y
|
|
||||||
// you must call mousecam_reset() after this if you want to go back to normal operation
|
|
||||||
int mousecam_frame_capture(byte *pdata)
|
|
||||||
{
|
|
||||||
mousecam_write_reg(ADNS3080_FRAME_CAPTURE,0x83);
|
|
||||||
|
|
||||||
digitalWrite(PIN_MOUSECAM_CS, LOW);
|
|
||||||
|
|
||||||
SPI.transfer(ADNS3080_PIXEL_BURST);
|
|
||||||
delayMicroseconds(50);
|
|
||||||
|
|
||||||
int pix;
|
|
||||||
byte started = 0;
|
|
||||||
int count;
|
|
||||||
int timeout = 0;
|
|
||||||
int ret = 0;
|
|
||||||
for(count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y; )
|
|
||||||
{
|
|
||||||
pix = SPI.transfer(0xff);
|
|
||||||
delayMicroseconds(10);
|
|
||||||
if(started==0)
|
|
||||||
{
|
|
||||||
if(pix&0x40)
|
|
||||||
started = 1;
|
|
||||||
else
|
|
||||||
{
|
|
||||||
timeout++;
|
|
||||||
if(timeout==100)
|
|
||||||
{
|
|
||||||
ret = -1;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if(started==1)
|
|
||||||
{
|
|
||||||
pdata[count++] = (pix & 0x3f)<<2; // scale to normal grayscale byte range
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
digitalWrite(PIN_MOUSECAM_CS,HIGH);
|
|
||||||
delayMicroseconds(14);
|
|
||||||
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
//-------------------------------------------------------OPTICAL SENSOR CODE END------------------------------------------------------//
|
|
||||||
|
|
||||||
//Tracker Variables
|
|
||||||
int current_x = 0;
|
|
||||||
int current_y = 0;
|
|
||||||
int goal_x = 0;
|
|
||||||
int goal_y = 0;
|
|
||||||
int distanceGoal;
|
|
||||||
bool commandComplete = 1;
|
|
||||||
float powerUsage_mWh = 0;
|
|
||||||
int distTravelled_mm = 0;
|
|
||||||
bool initialAngleSet=false;
|
|
||||||
|
|
||||||
//calibration varibles
|
|
||||||
int angularDrift = 0; //+ve to right, -ve to left
|
|
||||||
int leftStart = 80; //pwm min for left motor
|
|
||||||
int leftStop = 255; //pwm max for left motor
|
|
||||||
int rightStart = 80; //pwm min for right motor
|
|
||||||
int rightStop = 255; //pwm max for right motor
|
|
||||||
|
|
||||||
|
|
||||||
//Energy Usage Variables
|
|
||||||
unsigned long previousMillis_Energy = 0; // will store last time energy use was updated
|
|
||||||
const long interval_Energy = 1000; //energy usaged update frequency
|
|
||||||
float totalEnergyUsed = 0;
|
|
||||||
float powerUsed = 0;
|
|
||||||
int loopCount = 0;
|
|
||||||
float motorVoltage = 0;
|
|
||||||
|
|
||||||
int getPWMfromSpeed(float speedr,bool left) {
|
|
||||||
if (speedr >= 1) {
|
|
||||||
return 512;
|
|
||||||
}
|
|
||||||
else if (speedr < 0) {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
int speedpercentage = (speedr * 100);
|
|
||||||
if(left){
|
|
||||||
return map(speedpercentage,0,100,leftStart,leftStop);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
return map(speedpercentage,0,100,rightStart,rightStop);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup()
|
|
||||||
{
|
|
||||||
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
|
|
||||||
//************************** Motor Pins Defining **************************//
|
|
||||||
pinMode(DIRR, OUTPUT);
|
|
||||||
pinMode(DIRL, OUTPUT);
|
|
||||||
pinMode(pwmr, OUTPUT);
|
|
||||||
pinMode(pwml, OUTPUT);
|
|
||||||
digitalWrite(pwmr, HIGH); //setting right motor speed at maximum
|
|
||||||
digitalWrite(pwml, HIGH); //setting left motor speed at maximum
|
|
||||||
//*******************************************************************//
|
|
||||||
|
|
||||||
//Basic pin setups
|
|
||||||
|
|
||||||
noInterrupts(); //disable all interrupts
|
|
||||||
pinMode(13, OUTPUT); //Pin13 is used to time the loops of the controller
|
|
||||||
pinMode(3, INPUT_PULLUP); //Pin3 is the input from the Buck/Boost switch
|
|
||||||
pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch
|
|
||||||
analogReference(EXTERNAL); // We are using an external analogue reference for the ADC
|
|
||||||
|
|
||||||
// TimerA0 initialization for control-loop interrupt.
|
|
||||||
|
|
||||||
TCA0.SINGLE.PER = 999; //
|
|
||||||
TCA0.SINGLE.CMP1 = 999; //
|
|
||||||
TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M.
|
|
||||||
TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm;
|
|
||||||
|
|
||||||
// TimerB0 initialization for PWM output
|
|
||||||
|
|
||||||
pinMode(6, OUTPUT);
|
|
||||||
TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz
|
|
||||||
analogWrite(6, 120);
|
|
||||||
|
|
||||||
interrupts(); //enable interrupts.
|
|
||||||
Wire.begin(); // We need this for the i2c comms for the current sensor
|
|
||||||
ina219.init(); // this initiates the current sensor
|
|
||||||
Wire.setClock(700000); // set the comms speed for i2c
|
|
||||||
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
|
|
||||||
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
|
|
||||||
Serial4.begin(9600); // Set up software UART
|
|
||||||
|
|
||||||
//Serial.println(getPWMfromSpeed(-1));
|
|
||||||
//Serial.println(getPWMfromSpeed(256));
|
|
||||||
//Serial.println(getPWMfromSpeed(0.5));
|
|
||||||
// Other Drive setup stuff
|
|
||||||
/////////currentHeading = REQUEST HEADING HERE;
|
|
||||||
|
|
||||||
analogWrite(pwmr,0);
|
|
||||||
analogWrite(pwml,0);
|
|
||||||
//digitalWrite(DIRR, LOW);
|
|
||||||
//digitalWrite(DIRL, HIGH);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
pinMode(PIN_SS,OUTPUT);
|
|
||||||
pinMode(PIN_MISO,INPUT);
|
|
||||||
pinMode(PIN_MOSI,OUTPUT);
|
|
||||||
pinMode(PIN_SCK,OUTPUT);
|
|
||||||
|
|
||||||
SPI.begin();
|
|
||||||
SPI.setClockDivider(SPI_CLOCK_DIV32);
|
|
||||||
SPI.setDataMode(SPI_MODE3);
|
|
||||||
SPI.setBitOrder(MSBFIRST);
|
|
||||||
|
|
||||||
Serial.begin(9600);
|
|
||||||
|
|
||||||
if(mousecam_init()==-1)
|
|
||||||
{
|
|
||||||
Serial.println("Mouse cam failed to init");
|
|
||||||
while(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
bool commandCompletionStatus = 1; //0-No Command, 1-New Command, 2-Command being run, 3-Command Complete
|
|
||||||
int requiredHeading = 0;
|
|
||||||
int distance = 0;
|
|
||||||
float spd = 0;
|
|
||||||
int currentHeading = 0;
|
|
||||||
//reset variables for update on completion
|
|
||||||
|
|
||||||
unsigned long previousMillis_Command = 0;
|
|
||||||
const long interval_Command = 1000;
|
|
||||||
DeserializationError error;
|
|
||||||
|
|
||||||
char asciiart(int k)
|
|
||||||
{
|
|
||||||
static char foo[] = "WX86*3I>!;~:,`. ";
|
|
||||||
return foo[k>>4];
|
|
||||||
}
|
|
||||||
|
|
||||||
byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y];
|
|
||||||
DynamicJsonDocument rdoc(1024);
|
|
||||||
|
|
||||||
void loop()
|
|
||||||
{
|
|
||||||
|
|
||||||
//Serial.println("Here");
|
|
||||||
//deserializeJson(rdoc, Serial1); // Take JSON input from UART1
|
|
||||||
|
|
||||||
//Check for updates every 1s...
|
|
||||||
unsigned long currentMillis_Command = millis();
|
|
||||||
|
|
||||||
if (currentMillis_Command - previousMillis_Command >= interval_Command) {
|
|
||||||
// save the last time you blinked the LED
|
|
||||||
previousMillis_Command = currentMillis_Command;
|
|
||||||
// receive doc, not sure how big this needs to be
|
|
||||||
error = deserializeJson(rdoc, Serial4);
|
|
||||||
|
|
||||||
//Serial.println
|
|
||||||
Serial.println("I am here!");
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Test if parsing succeeds.
|
|
||||||
if (error) {
|
|
||||||
//Serial.print(F("deserializeJson() failed: "));
|
|
||||||
//Serial.println(error.f_str());
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
//parsing success, prepare command and pull request information
|
|
||||||
commandCompletionStatus = 1;
|
|
||||||
requiredHeading = rdoc["rH"];
|
|
||||||
distance = rdoc["dist"];
|
|
||||||
spd = rdoc["sp"];
|
|
||||||
currentHeading = rdoc["cH"];
|
|
||||||
//reset variables for update on completion
|
|
||||||
commandComplete = 0;
|
|
||||||
powerUsage_mWh = 0.0;
|
|
||||||
distTravelled_mm = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
//if(current_x!=goal_x)
|
|
||||||
|
|
||||||
// Do Drive stuff, set the 5 values above
|
|
||||||
|
|
||||||
if(commandCompletionStatus == 0){ //noCommand
|
|
||||||
//Do Nothing just wait
|
|
||||||
}
|
|
||||||
if(commandCompletionStatus == 1){ //newCommand
|
|
||||||
//set goals
|
|
||||||
goal_x += distance*sin(requiredHeading);
|
|
||||||
goal_y += distance*cos(requiredHeading);
|
|
||||||
total_y = 0;
|
|
||||||
total_x = 0;
|
|
||||||
commandCompletionStatus = 2;
|
|
||||||
|
|
||||||
initialAngleSet=false;
|
|
||||||
}
|
|
||||||
if(commandCompletionStatus == 2){ //ongoingCommand
|
|
||||||
//start moving towards goal
|
|
||||||
|
|
||||||
//set angle first
|
|
||||||
if(!initialAngleSet){
|
|
||||||
//turn to angle
|
|
||||||
currentHeading = getCurrentHeading();
|
|
||||||
if(currentHeading<requiredHeading){ //turn right
|
|
||||||
analogWrite(pwmr,getPWMfromSpeed(spd,false));
|
|
||||||
analogWrite(pwml,getPWMfromSpeed(spd,true));
|
|
||||||
digitalWrite(DIRR, LOW);
|
|
||||||
digitalWrite(DIRL, LOW);
|
|
||||||
}
|
|
||||||
else if(currentHeading>requiredHeading){ //turn left
|
|
||||||
analogWrite(pwmr,getPWMfromSpeed(spd,false));
|
|
||||||
analogWrite(pwml,getPWMfromSpeed(spd,true));
|
|
||||||
digitalWrite(DIRR, HIGH);
|
|
||||||
digitalWrite(DIRL, HIGH);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
//heading correct therefore move to next step...
|
|
||||||
//STOP!!!!
|
|
||||||
digitalWrite(pwmr,LOW);
|
|
||||||
digitalWrite(pwml,LOW);
|
|
||||||
|
|
||||||
initialAngleSet = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else{ //then move forwards but check angle for drift
|
|
||||||
if(total_x - distance > 0){ //go forwards
|
|
||||||
analogWrite(pwmr,getPWMfromSpeed(spd,false));
|
|
||||||
analogWrite(pwml,getPWMfromSpeed(spd,true));
|
|
||||||
digitalWrite(DIRR, HIGH);
|
|
||||||
digitalWrite(DIRL, LOW);
|
|
||||||
}
|
|
||||||
else if(total_x - distance < 0){//go backwards
|
|
||||||
analogWrite(pwmr,getPWMfromSpeed(spd,false));
|
|
||||||
analogWrite(pwml,getPWMfromSpeed(spd,true));
|
|
||||||
digitalWrite(DIRR, LOW);
|
|
||||||
digitalWrite(DIRL, HIGH);
|
|
||||||
}
|
|
||||||
else if((total_x == distance)) {//distance met
|
|
||||||
//STOP!!!!!
|
|
||||||
digitalWrite(pwmr,LOW);
|
|
||||||
digitalWrite(pwml,LOW);
|
|
||||||
commandCompletionStatus = 3;
|
|
||||||
initialAngleSet = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
if(commandCompletionStatus == 3){ //currentPosMatchesOrExceedsRequest
|
|
||||||
///finish moving
|
|
||||||
|
|
||||||
//send update via UART
|
|
||||||
|
|
||||||
//prepare feedback variables
|
|
||||||
commandComplete = true;
|
|
||||||
current_x = goal_x;
|
|
||||||
current_y = goal_y;
|
|
||||||
distTravelled_mm += distance;
|
|
||||||
//compile energy use
|
|
||||||
unsigned long currentMillis_Energy = millis();
|
|
||||||
|
|
||||||
totalEnergyUsed += (currentMillis_Energy - previousMillis_Energy) * (powerUsed / loopCount) / 1000 / (60 * 60);
|
|
||||||
previousMillis_Energy = currentMillis_Energy;
|
|
||||||
if (debug) {
|
|
||||||
Serial.print(motorVoltage);
|
|
||||||
Serial.print("Energy Used: ");
|
|
||||||
Serial.print(totalEnergyUsed);
|
|
||||||
Serial.println("mWh");
|
|
||||||
}
|
|
||||||
|
|
||||||
loopCount = 0; //reset counter to zero
|
|
||||||
powerUsed = 0;//reset power usage
|
|
||||||
powerUsage_mWh = totalEnergyUsed;
|
|
||||||
totalEnergyUsed = 0;
|
|
||||||
|
|
||||||
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
|
|
||||||
tdoc["comp"] = commandComplete;
|
|
||||||
tdoc["mWh"] = powerUsage_mWh;
|
|
||||||
tdoc["mm"] = distTravelled_mm;
|
|
||||||
tdoc["pos"][0] = current_x;
|
|
||||||
tdoc["pos"][1] = current_y;
|
|
||||||
serializeJson(tdoc, Serial1); // Build JSON and send on UART1
|
|
||||||
commandCompletionStatus = 0;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
//Handle power usage
|
|
||||||
//find motor voltage
|
|
||||||
//int motorVSensor = analogRead(A5);
|
|
||||||
//float motorVoltage = motorVSensor * (5.0 / 1023.0);
|
|
||||||
float motorVoltage = 5;
|
|
||||||
|
|
||||||
//find average power
|
|
||||||
|
|
||||||
if(current_mA >=0){
|
|
||||||
powerUsed += current_mA * motorVoltage;
|
|
||||||
}
|
|
||||||
Serial.println(powerUsed);
|
|
||||||
|
|
||||||
//calculate averages for energy use calculations
|
|
||||||
loopCount += 1; //handle loop quantity for averaging
|
|
||||||
|
|
||||||
//find average current
|
|
||||||
//find average voltage
|
|
||||||
|
|
||||||
//update command/control
|
|
||||||
|
|
||||||
|
|
||||||
if(movementflag){
|
|
||||||
|
|
||||||
tdistance = tdistance + convTwosComp(xydat[0]);
|
|
||||||
Serial.println("Distance = " + String(tdistance));
|
|
||||||
movementflag=0;
|
|
||||||
delay(3);
|
|
||||||
}
|
|
||||||
int val = mousecam_read_reg(ADNS3080_PIXEL_SUM);
|
|
||||||
MD md;
|
|
||||||
mousecam_read_motion(&md);
|
|
||||||
for(int i=0; i<md.squal/4; i++)
|
|
||||||
Serial.print('*');
|
|
||||||
Serial.print(' ');
|
|
||||||
Serial.print((val*100)/351);
|
|
||||||
Serial.print(' ');
|
|
||||||
Serial.print(md.shutter); Serial.print(" (");
|
|
||||||
Serial.print((int)md.dx); Serial.print(',');
|
|
||||||
Serial.print((int)md.dy); Serial.println(')');
|
|
||||||
|
|
||||||
// Serial.println(md.max_pix);
|
|
||||||
delay(100);
|
|
||||||
|
|
||||||
|
|
||||||
distance_x = md.dx; //convTwosComp(md.dx);
|
|
||||||
distance_y = md.dy; //convTwosComp(md.dy);
|
|
||||||
|
|
||||||
total_x1 = (total_x1 + distance_x);
|
|
||||||
total_y1 = (total_y1 + distance_y);
|
|
||||||
|
|
||||||
total_x = 10*total_x1/157; //Conversion from counts per inch to mm (400 counts per inch)
|
|
||||||
total_y = 10*total_y1/157; //Conversion from counts per inch to mm (400 counts per inch)
|
|
||||||
|
|
||||||
|
|
||||||
Serial.print('\n');
|
|
||||||
|
|
||||||
|
|
||||||
Serial.println("Distance_x = " + String(total_x));
|
|
||||||
|
|
||||||
Serial.println("Distance_y = " + String(total_y));
|
|
||||||
Serial.print('\n');
|
|
||||||
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
|
|
||||||
unsigned long currentMillis = millis();
|
|
||||||
if (loopTrigger) { // This loop is triggered, it wont run unless there is an interrupt
|
|
||||||
|
|
||||||
digitalWrite(13, HIGH); // set pin 13. Pin13 shows the time consumed by each control cycle. It's used for debugging.
|
|
||||||
|
|
||||||
// Sample all of the measurements and check which control mode we are in
|
|
||||||
sampling();
|
|
||||||
CL_mode = digitalRead(3); // input from the OL_CL switch
|
|
||||||
Boost_mode = digitalRead(2); // input from the Buck_Boost switch
|
|
||||||
|
|
||||||
if (Boost_mode) {
|
|
||||||
if (CL_mode) { //Closed Loop Boost
|
|
||||||
pwm_modulate(1); // This disables the Boost as we are not using this mode
|
|
||||||
} else { // Open Loop Boost
|
|
||||||
pwm_modulate(1); // This disables the Boost as we are not using this mode
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
if (CL_mode) { // Closed Loop Buck
|
|
||||||
// The closed loop path has a voltage controller cascaded with a current controller. The voltage controller
|
|
||||||
// creates a current demand based upon the voltage error. This demand is saturated to give current limiting.
|
|
||||||
// The current loop then gives a duty cycle demand based upon the error between demanded current and measured
|
|
||||||
// current
|
|
||||||
current_limit = 3; // Buck has a higher current limit
|
|
||||||
ev = vref - vb; //voltage error at this time
|
|
||||||
cv = pidv(ev); //voltage pid
|
|
||||||
cv = saturation(cv, current_limit, 0); //current demand saturation
|
|
||||||
ei = cv - iL; //current error
|
|
||||||
closed_loop = pidi(ei); //current pid
|
|
||||||
closed_loop = saturation(closed_loop, 0.99, 0.01); //duty_cycle saturation
|
|
||||||
pwm_modulate(closed_loop); //pwm modulation
|
|
||||||
} else { // Open Loop Buck
|
|
||||||
current_limit = 3; // Buck has a higher current limit
|
|
||||||
oc = iL - current_limit; // Calculate the difference between current measurement and current limit
|
|
||||||
if ( oc > 0) {
|
|
||||||
open_loop = open_loop - 0.001; // We are above the current limit so less duty cycle
|
|
||||||
} else {
|
|
||||||
open_loop = open_loop + 0.001; // We are below the current limit so more duty cycle
|
|
||||||
}
|
|
||||||
open_loop = saturation(open_loop, dutyref, 0.02); // saturate the duty cycle at the reference or a min of 0.01
|
|
||||||
pwm_modulate(open_loop); // and send it out
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// closed loop control path
|
|
||||||
|
|
||||||
digitalWrite(13, LOW); // reset pin13.
|
|
||||||
loopTrigger = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
//************************** Motor Testing **************************//
|
|
||||||
//this part of the code decides the direction of motor rotations depending on the time lapsed. currentMillis records the time lapsed once it is called.
|
|
||||||
|
|
||||||
|
|
||||||
//*******************************************************************//
|
|
||||||
//----------------s---------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
int getCurrentHeading(){
|
|
||||||
int currentAngle = 0; //-------------___<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
|
|
||||||
return currentAngle;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Timer A CMP1 interrupt. Every 800us the program enters this interrupt.
|
|
||||||
// This, clears the incoming interrupt flag and triggers the main loop.
|
|
||||||
|
|
||||||
ISR(TCA0_CMP1_vect) {
|
|
||||||
TCA0.SINGLE.INTFLAGS |= TCA_SINGLE_CMP1_bm; //clear interrupt flag
|
|
||||||
loopTrigger = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
// This subroutine processes all of the analogue samples, creating the required values for the main loop
|
|
||||||
|
|
||||||
void sampling() {
|
|
||||||
|
|
||||||
// Make the initial sampling operations for the circuit measurements
|
|
||||||
|
|
||||||
sensorValue0 = analogRead(A0); //sample Vb
|
|
||||||
sensorValue2 = analogRead(A2); //sample Vref
|
|
||||||
sensorValue3 = analogRead(A3); //sample Vpd
|
|
||||||
current_mA = ina219.getCurrent_mA(); // sample the inductor current (via the sensor chip)
|
|
||||||
|
|
||||||
// Process the values so they are a bit more usable/readable
|
|
||||||
// The analogRead process gives a value between 0 and 1023
|
|
||||||
// representing a voltage between 0 and the analogue reference which is 4.096V
|
|
||||||
|
|
||||||
vb = sensorValue0 * (4.096 / 1023.0); // Convert the Vb sensor reading to volts
|
|
||||||
vref = sensorValue2 * (4.096 / 1023.0); // Convert the Vref sensor reading to volts
|
|
||||||
vpd = sensorValue3 * (4.096 / 1023.0); // Convert the Vpd sensor reading to volts
|
|
||||||
|
|
||||||
// The inductor current is in mA from the sensor so we need to convert to amps.
|
|
||||||
// We want to treat it as an input current in the Boost, so its also inverted
|
|
||||||
// For open loop control the duty cycle reference is calculated from the sensor
|
|
||||||
// differently from the Vref, this time scaled between zero and 1.
|
|
||||||
// The boost duty cycle needs to be saturated with a 0.33 minimum to prevent high output voltages
|
|
||||||
|
|
||||||
if (Boost_mode == 1) {
|
|
||||||
iL = -current_mA / 1000.0;
|
|
||||||
dutyref = saturation(sensorValue2 * (1.0 / 1023.0), 0.99, 0.33);
|
|
||||||
} else {
|
|
||||||
iL = current_mA / 1000.0;
|
|
||||||
dutyref = sensorValue2 * (1.0 / 1023.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
float saturation( float sat_input, float uplim, float lowlim) { // Saturatio function
|
|
||||||
if (sat_input > uplim) sat_input = uplim;
|
|
||||||
else if (sat_input < lowlim ) sat_input = lowlim;
|
|
||||||
else;
|
|
||||||
return sat_input;
|
|
||||||
}
|
|
||||||
|
|
||||||
void pwm_modulate(float pwm_input) { // PWM function
|
|
||||||
analogWrite(6, (int)(255 - pwm_input * 255));
|
|
||||||
}
|
|
||||||
|
|
||||||
// This is a PID controller for the voltage
|
|
||||||
|
|
||||||
float pidv( float pid_input) {
|
|
||||||
float e_integration;
|
|
||||||
e0v = pid_input;
|
|
||||||
e_integration = e0v;
|
|
||||||
|
|
||||||
//anti-windup, if last-time pid output reaches the limitation, this time there won't be any intergrations.
|
|
||||||
if (u1v >= uv_max) {
|
|
||||||
e_integration = 0;
|
|
||||||
} else if (u1v <= uv_min) {
|
|
||||||
e_integration = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
delta_uv = kpv * (e0v - e1v) + kiv * Ts * e_integration + kdv / Ts * (e0v - 2 * e1v + e2v); //incremental PID programming avoids integrations.there is another PID program called positional PID.
|
|
||||||
u0v = u1v + delta_uv; //this time's control output
|
|
||||||
|
|
||||||
//output limitation
|
|
||||||
saturation(u0v, uv_max, uv_min);
|
|
||||||
|
|
||||||
u1v = u0v; //update last time's control output
|
|
||||||
e2v = e1v; //update last last time's error
|
|
||||||
e1v = e0v; // update last time's error
|
|
||||||
return u0v;
|
|
||||||
}
|
|
||||||
|
|
||||||
// This is a PID controller for the current
|
|
||||||
|
|
||||||
float pidi(float pid_input) {
|
|
||||||
float e_integration;
|
|
||||||
e0i = pid_input;
|
|
||||||
e_integration = e0i;
|
|
||||||
|
|
||||||
//anti-windup
|
|
||||||
if (u1i >= ui_max) {
|
|
||||||
e_integration = 0;
|
|
||||||
} else if (u1i <= ui_min) {
|
|
||||||
e_integration = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
delta_ui = kpi * (e0i - e1i) + kii * Ts * e_integration + kdi / Ts * (e0i - 2 * e1i + e2i); //incremental PID programming avoids integrations.
|
|
||||||
u0i = u1i + delta_ui; //this time's control output
|
|
||||||
|
|
||||||
//output limitation
|
|
||||||
saturation(u0i, ui_max, ui_min);
|
|
||||||
|
|
||||||
u1i = u0i; //update last time's control output
|
|
||||||
e2i = e1i; //update last last time's error
|
|
||||||
e1i = e0i; // update last time's error
|
|
||||||
return u0i;
|
|
||||||
}
|
|
||||||
39
Drive/include/README
Normal file
39
Drive/include/README
Normal file
|
|
@ -0,0 +1,39 @@
|
||||||
|
|
||||||
|
This directory is intended for project header files.
|
||||||
|
|
||||||
|
A header file is a file containing C declarations and macro definitions
|
||||||
|
to be shared between several project source files. You request the use of a
|
||||||
|
header file in your project source file (C, C++, etc) located in `src` folder
|
||||||
|
by including it, with the C preprocessing directive `#include'.
|
||||||
|
|
||||||
|
```src/main.c
|
||||||
|
|
||||||
|
#include "header.h"
|
||||||
|
|
||||||
|
int main (void)
|
||||||
|
{
|
||||||
|
...
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
Including a header file produces the same results as copying the header file
|
||||||
|
into each source file that needs it. Such copying would be time-consuming
|
||||||
|
and error-prone. With a header file, the related declarations appear
|
||||||
|
in only one place. If they need to be changed, they can be changed in one
|
||||||
|
place, and programs that include the header file will automatically use the
|
||||||
|
new version when next recompiled. The header file eliminates the labor of
|
||||||
|
finding and changing all the copies as well as the risk that a failure to
|
||||||
|
find one copy will result in inconsistencies within a program.
|
||||||
|
|
||||||
|
In C, the usual convention is to give header files names that end with `.h'.
|
||||||
|
It is most portable to use only letters, digits, dashes, and underscores in
|
||||||
|
header file names, and at most one dot.
|
||||||
|
|
||||||
|
Read more about using header files in official GCC documentation:
|
||||||
|
|
||||||
|
* Include Syntax
|
||||||
|
* Include Operation
|
||||||
|
* Once-Only Headers
|
||||||
|
* Computed Includes
|
||||||
|
|
||||||
|
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
|
||||||
46
Drive/lib/README
Normal file
46
Drive/lib/README
Normal file
|
|
@ -0,0 +1,46 @@
|
||||||
|
|
||||||
|
This directory is intended for project specific (private) libraries.
|
||||||
|
PlatformIO will compile them to static libraries and link into executable file.
|
||||||
|
|
||||||
|
The source code of each library should be placed in a an own separate directory
|
||||||
|
("lib/your_library_name/[here are source files]").
|
||||||
|
|
||||||
|
For example, see a structure of the following two libraries `Foo` and `Bar`:
|
||||||
|
|
||||||
|
|--lib
|
||||||
|
| |
|
||||||
|
| |--Bar
|
||||||
|
| | |--docs
|
||||||
|
| | |--examples
|
||||||
|
| | |--src
|
||||||
|
| | |- Bar.c
|
||||||
|
| | |- Bar.h
|
||||||
|
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
|
||||||
|
| |
|
||||||
|
| |--Foo
|
||||||
|
| | |- Foo.c
|
||||||
|
| | |- Foo.h
|
||||||
|
| |
|
||||||
|
| |- README --> THIS FILE
|
||||||
|
|
|
||||||
|
|- platformio.ini
|
||||||
|
|--src
|
||||||
|
|- main.c
|
||||||
|
|
||||||
|
and a contents of `src/main.c`:
|
||||||
|
```
|
||||||
|
#include <Foo.h>
|
||||||
|
#include <Bar.h>
|
||||||
|
|
||||||
|
int main (void)
|
||||||
|
{
|
||||||
|
...
|
||||||
|
}
|
||||||
|
|
||||||
|
```
|
||||||
|
|
||||||
|
PlatformIO Library Dependency Finder will find automatically dependent
|
||||||
|
libraries scanning project source files.
|
||||||
|
|
||||||
|
More information about PlatformIO Library Dependency Finder
|
||||||
|
- https://docs.platformio.org/page/librarymanager/ldf.html
|
||||||
21
Drive/platformio.ini
Normal file
21
Drive/platformio.ini
Normal file
|
|
@ -0,0 +1,21 @@
|
||||||
|
; PlatformIO Project Configuration File
|
||||||
|
;
|
||||||
|
; Build options: build flags, source filter
|
||||||
|
; Upload options: custom upload port, speed and extra flags
|
||||||
|
; Library options: dependencies, extra library storages
|
||||||
|
; Advanced options: extra scripting
|
||||||
|
;
|
||||||
|
; Please visit documentation for the other options and examples
|
||||||
|
; https://docs.platformio.org/page/projectconf.html
|
||||||
|
|
||||||
|
[env]
|
||||||
|
lib_deps =
|
||||||
|
bblanchon/ArduinoJson @ ^6.18.0
|
||||||
|
wollewald/INA219_WE @ ^1.1.6
|
||||||
|
monitor_speed = 115200
|
||||||
|
upload_speed = 115200
|
||||||
|
|
||||||
|
[env:nano_every]
|
||||||
|
platform = atmelmegaavr
|
||||||
|
board = nano_every
|
||||||
|
framework = arduino
|
||||||
805
Drive/src/main.cpp
Normal file
805
Drive/src/main.cpp
Normal file
|
|
@ -0,0 +1,805 @@
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include <ArduinoJson.h>
|
||||||
|
// #include <string>
|
||||||
|
#include <Wire.h>
|
||||||
|
#include <INA219_WE.h>
|
||||||
|
#include "SPI.h"
|
||||||
|
#include <SoftwareSerial.h>
|
||||||
|
|
||||||
|
// #define RXpin 4 // Define your RX pin here
|
||||||
|
// #define TXpin 13 // Define your TX pin here
|
||||||
|
|
||||||
|
// SoftwareSerial mySerial(RXpin, TXpin);
|
||||||
|
|
||||||
|
bool debug = false;
|
||||||
|
|
||||||
|
//TO IMPLEMENT
|
||||||
|
//DONE 2 way serial
|
||||||
|
//DONE F<>,B<>,S,L<>,R<>,p<0--1023>
|
||||||
|
//DONE Obtain current and power usage, get voltage from analog pin
|
||||||
|
//request angle facing
|
||||||
|
//DONE speed control 0-1
|
||||||
|
//speed calibration, 0 stop and max speed to match
|
||||||
|
//distance travveled and x and y at request
|
||||||
|
|
||||||
|
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
|
||||||
|
INA219_WE ina219; // this is the instantiation of the library for the current sensor
|
||||||
|
|
||||||
|
float open_loop, closed_loop; // Duty Cycles
|
||||||
|
float vpd, vb, vref, iL, dutyref, current_mA; // Measurement Variables
|
||||||
|
unsigned int sensorValue0, sensorValue1, sensorValue2, sensorValue3; // ADC sample values declaration
|
||||||
|
float ev = 0, cv = 0, ei = 0, oc = 0; //internal signals
|
||||||
|
float Ts = 0.0008; //1.25 kHz control frequency. It's better to design the control period as integral multiple of switching period.
|
||||||
|
float kpv = 0.05024, kiv = 15.78, kdv = 0; // voltage pid.
|
||||||
|
float u0v, u1v, delta_uv, e0v, e1v, e2v; // u->output; e->error; 0->this time; 1->last time; 2->last last time
|
||||||
|
float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid.
|
||||||
|
float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller
|
||||||
|
float uv_max = 4, uv_min = 0; //anti-windup limitation
|
||||||
|
float ui_max = 1, ui_min = 0; //anti-windup limitation
|
||||||
|
float current_limit = 1.0;
|
||||||
|
boolean Boost_mode = 0;
|
||||||
|
boolean CL_mode = 0;
|
||||||
|
|
||||||
|
unsigned int loopTrigger;
|
||||||
|
unsigned int com_count = 0; // a variables to count the interrupts. Used for program debugging.
|
||||||
|
|
||||||
|
//************************** Motor Constants **************************//
|
||||||
|
unsigned long previousMillis = 0; //initializing time counter
|
||||||
|
const long f_i = 10000; //time to move in forward direction, please calculate the precision and conversion factor
|
||||||
|
const long r_i = 20000; //time to rotate clockwise
|
||||||
|
const long b_i = 30000; //time to move backwards
|
||||||
|
const long l_i = 40000; //time to move anticlockwise
|
||||||
|
const long s_i = 50000;
|
||||||
|
int DIRRstate = LOW; //initializing direction states
|
||||||
|
int DIRLstate = HIGH;
|
||||||
|
|
||||||
|
int DIRL = 20; //defining left direction pin
|
||||||
|
int DIRR = 21; //defining right direction pin
|
||||||
|
|
||||||
|
int pwmr = 5; //pin to control right wheel speed using pwm
|
||||||
|
int pwml = 9; //pin to control left wheel speed using pwm
|
||||||
|
//*******************************************************************//
|
||||||
|
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
|
||||||
|
|
||||||
|
//-------------------------------------------------------OPTICAL SENSOR CODE START------------------------------------------------------//
|
||||||
|
#define PIN_SS 10
|
||||||
|
#define PIN_MISO 12
|
||||||
|
#define PIN_MOSI 11
|
||||||
|
#define PIN_SCK 13
|
||||||
|
|
||||||
|
#define PIN_MOUSECAM_RESET 8
|
||||||
|
#define PIN_MOUSECAM_CS 7
|
||||||
|
|
||||||
|
#define ADNS3080_PIXELS_X 30
|
||||||
|
#define ADNS3080_PIXELS_Y 30
|
||||||
|
|
||||||
|
#define ADNS3080_PRODUCT_ID 0x00
|
||||||
|
#define ADNS3080_REVISION_ID 0x01
|
||||||
|
#define ADNS3080_MOTION 0x02
|
||||||
|
#define ADNS3080_DELTA_X 0x03
|
||||||
|
#define ADNS3080_DELTA_Y 0x04
|
||||||
|
#define ADNS3080_SQUAL 0x05
|
||||||
|
#define ADNS3080_PIXEL_SUM 0x06
|
||||||
|
#define ADNS3080_MAXIMUM_PIXEL 0x07
|
||||||
|
#define ADNS3080_CONFIGURATION_BITS 0x0a
|
||||||
|
#define ADNS3080_EXTENDED_CONFIG 0x0b
|
||||||
|
#define ADNS3080_DATA_OUT_LOWER 0x0c
|
||||||
|
#define ADNS3080_DATA_OUT_UPPER 0x0d
|
||||||
|
#define ADNS3080_SHUTTER_LOWER 0x0e
|
||||||
|
#define ADNS3080_SHUTTER_UPPER 0x0f
|
||||||
|
#define ADNS3080_FRAME_PERIOD_LOWER 0x10
|
||||||
|
#define ADNS3080_FRAME_PERIOD_UPPER 0x11
|
||||||
|
#define ADNS3080_MOTION_CLEAR 0x12
|
||||||
|
#define ADNS3080_FRAME_CAPTURE 0x13
|
||||||
|
#define ADNS3080_SROM_ENABLE 0x14
|
||||||
|
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19
|
||||||
|
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a
|
||||||
|
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b
|
||||||
|
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c
|
||||||
|
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e
|
||||||
|
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e
|
||||||
|
#define ADNS3080_SROM_ID 0x1f
|
||||||
|
#define ADNS3080_OBSERVATION 0x3d
|
||||||
|
#define ADNS3080_INVERSE_PRODUCT_ID 0x3f
|
||||||
|
#define ADNS3080_PIXEL_BURST 0x40
|
||||||
|
#define ADNS3080_MOTION_BURST 0x50
|
||||||
|
#define ADNS3080_SROM_LOAD 0x60
|
||||||
|
|
||||||
|
#define ADNS3080_PRODUCT_ID_VAL 0x17
|
||||||
|
|
||||||
|
int total_x = 0;
|
||||||
|
int total_y = 0;
|
||||||
|
|
||||||
|
int total_x1 = 0;
|
||||||
|
int total_y1 = 0;
|
||||||
|
|
||||||
|
int x = 0;
|
||||||
|
int y = 0;
|
||||||
|
|
||||||
|
int a = 0;
|
||||||
|
int b = 0;
|
||||||
|
|
||||||
|
int distance_x = 0;
|
||||||
|
int distance_y = 0;
|
||||||
|
|
||||||
|
volatile byte movementflag = 0;
|
||||||
|
volatile int xydat[2];
|
||||||
|
|
||||||
|
// FUNCTION DELCARATIONS //
|
||||||
|
|
||||||
|
float pidi(float pid_input);
|
||||||
|
float pidv(float pid_input);
|
||||||
|
void pwm_modulate(float pwm_input);
|
||||||
|
float saturation(float sat_input, float uplim, float lowlim);
|
||||||
|
void sampling();
|
||||||
|
void mousecam_write_reg(int reg, int val);
|
||||||
|
int mousecam_read_reg(int reg);
|
||||||
|
void mousecam_reset();
|
||||||
|
int getCurrentHeading();
|
||||||
|
|
||||||
|
int convTwosComp(int b)
|
||||||
|
{
|
||||||
|
//Convert from 2's complement
|
||||||
|
if (b & 0x80)
|
||||||
|
{
|
||||||
|
b = -1 * ((b ^ 0xff) + 1);
|
||||||
|
}
|
||||||
|
return b;
|
||||||
|
}
|
||||||
|
|
||||||
|
int tdistance = 0;
|
||||||
|
|
||||||
|
void mousecam_reset(){
|
||||||
|
digitalWrite(PIN_MOUSECAM_RESET, HIGH);
|
||||||
|
delay(1); // reset pulse >10us
|
||||||
|
digitalWrite(PIN_MOUSECAM_RESET, LOW);
|
||||||
|
delay(35); // 35ms from reset to functional
|
||||||
|
}
|
||||||
|
|
||||||
|
int mousecam_init(){
|
||||||
|
pinMode(PIN_MOUSECAM_RESET, OUTPUT);
|
||||||
|
pinMode(PIN_MOUSECAM_CS, OUTPUT);
|
||||||
|
|
||||||
|
digitalWrite(PIN_MOUSECAM_CS, HIGH);
|
||||||
|
|
||||||
|
mousecam_reset();
|
||||||
|
|
||||||
|
int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID);
|
||||||
|
if (pid != ADNS3080_PRODUCT_ID_VAL)
|
||||||
|
return -1;
|
||||||
|
|
||||||
|
// turn on sensitive mode
|
||||||
|
mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void mousecam_write_reg(int reg, int val){
|
||||||
|
digitalWrite(PIN_MOUSECAM_CS, LOW);
|
||||||
|
SPI.transfer(reg | 0x80);
|
||||||
|
SPI.transfer(val);
|
||||||
|
digitalWrite(PIN_MOUSECAM_CS, HIGH);
|
||||||
|
delayMicroseconds(50);
|
||||||
|
}
|
||||||
|
|
||||||
|
int mousecam_read_reg(int reg){
|
||||||
|
digitalWrite(PIN_MOUSECAM_CS, LOW);
|
||||||
|
SPI.transfer(reg);
|
||||||
|
delayMicroseconds(75);
|
||||||
|
int ret = SPI.transfer(0xff);
|
||||||
|
digitalWrite(PIN_MOUSECAM_CS, HIGH);
|
||||||
|
delayMicroseconds(1);
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
struct MD{
|
||||||
|
byte motion;
|
||||||
|
char dx, dy;
|
||||||
|
byte squal;
|
||||||
|
word shutter;
|
||||||
|
byte max_pix;
|
||||||
|
};
|
||||||
|
|
||||||
|
void mousecam_read_motion(struct MD *p){
|
||||||
|
digitalWrite(PIN_MOUSECAM_CS, LOW);
|
||||||
|
SPI.transfer(ADNS3080_MOTION_BURST);
|
||||||
|
delayMicroseconds(75);
|
||||||
|
p->motion = SPI.transfer(0xff);
|
||||||
|
p->dx = SPI.transfer(0xff);
|
||||||
|
p->dy = SPI.transfer(0xff);
|
||||||
|
p->squal = SPI.transfer(0xff);
|
||||||
|
p->shutter = SPI.transfer(0xff) << 8;
|
||||||
|
p->shutter |= SPI.transfer(0xff);
|
||||||
|
p->max_pix = SPI.transfer(0xff);
|
||||||
|
digitalWrite(PIN_MOUSECAM_CS, HIGH);
|
||||||
|
delayMicroseconds(5);
|
||||||
|
}
|
||||||
|
|
||||||
|
// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y
|
||||||
|
// you must call mousecam_reset() after this if you want to go back to normal operation
|
||||||
|
int mousecam_frame_capture(byte *pdata)
|
||||||
|
{
|
||||||
|
mousecam_write_reg(ADNS3080_FRAME_CAPTURE, 0x83);
|
||||||
|
|
||||||
|
digitalWrite(PIN_MOUSECAM_CS, LOW);
|
||||||
|
|
||||||
|
SPI.transfer(ADNS3080_PIXEL_BURST);
|
||||||
|
delayMicroseconds(50);
|
||||||
|
|
||||||
|
int pix;
|
||||||
|
byte started = 0;
|
||||||
|
int count;
|
||||||
|
int timeout = 0;
|
||||||
|
int ret = 0;
|
||||||
|
for (count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y;)
|
||||||
|
{
|
||||||
|
pix = SPI.transfer(0xff);
|
||||||
|
delayMicroseconds(10);
|
||||||
|
if (started == 0)
|
||||||
|
{
|
||||||
|
if (pix & 0x40)
|
||||||
|
started = 1;
|
||||||
|
else
|
||||||
|
{
|
||||||
|
timeout++;
|
||||||
|
if (timeout == 100)
|
||||||
|
{
|
||||||
|
ret = -1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (started == 1)
|
||||||
|
{
|
||||||
|
pdata[count++] = (pix & 0x3f) << 2; // scale to normal grayscale byte range
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
digitalWrite(PIN_MOUSECAM_CS, HIGH);
|
||||||
|
delayMicroseconds(14);
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
//-------------------------------------------------------OPTICAL SENSOR CODE END------------------------------------------------------//
|
||||||
|
|
||||||
|
//Tracker Variables
|
||||||
|
int current_x = 0;
|
||||||
|
int current_y = 0;
|
||||||
|
int goal_x = 0;
|
||||||
|
int goal_y = 0;
|
||||||
|
int distanceGoal;
|
||||||
|
bool commandComplete = 1;
|
||||||
|
float powerUsage_mWh = 0;
|
||||||
|
int distTravelled_mm = 0;
|
||||||
|
bool initialAngleSet = false;
|
||||||
|
|
||||||
|
//calibration varibles
|
||||||
|
int angularDrift = 0; //+ve to right, -ve to left
|
||||||
|
int leftStart = 80; //pwm min for left motor
|
||||||
|
int leftStop = 255; //pwm max for left motor
|
||||||
|
int rightStart = 80; //pwm min for right motor
|
||||||
|
int rightStop = 255; //pwm max for right motor
|
||||||
|
|
||||||
|
//Energy Usage Variables
|
||||||
|
unsigned long previousMillis_Energy = 0; // will store last time energy use was updated
|
||||||
|
const long interval_Energy = 1000; //energy usaged update frequency
|
||||||
|
float totalEnergyUsed = 0;
|
||||||
|
float powerUsed = 0;
|
||||||
|
int loopCount = 0;
|
||||||
|
float motorVoltage = 0;
|
||||||
|
|
||||||
|
int getPWMfromSpeed(float speedr, bool left)
|
||||||
|
{
|
||||||
|
if (speedr >= 1)
|
||||||
|
{
|
||||||
|
return 512;
|
||||||
|
}
|
||||||
|
else if (speedr < 0)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
int speedpercentage = (speedr * 100);
|
||||||
|
if (left)
|
||||||
|
{
|
||||||
|
return map(speedpercentage, 0, 100, leftStart, leftStop);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return map(speedpercentage, 0, 100, rightStart, rightStop);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup()
|
||||||
|
{
|
||||||
|
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
|
||||||
|
//************************** Motor Pins Defining **************************//
|
||||||
|
pinMode(DIRR, OUTPUT);
|
||||||
|
pinMode(DIRL, OUTPUT);
|
||||||
|
pinMode(pwmr, OUTPUT);
|
||||||
|
pinMode(pwml, OUTPUT);
|
||||||
|
digitalWrite(pwmr, HIGH); //setting right motor speed at maximum
|
||||||
|
digitalWrite(pwml, HIGH); //setting left motor speed at maximum
|
||||||
|
//*******************************************************************//
|
||||||
|
|
||||||
|
//Basic pin setups
|
||||||
|
|
||||||
|
noInterrupts(); //disable all interrupts
|
||||||
|
pinMode(13, OUTPUT); //Pin13 is used to time the loops of the controller
|
||||||
|
pinMode(3, INPUT_PULLUP); //Pin3 is the input from the Buck/Boost switch
|
||||||
|
pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch
|
||||||
|
analogReference(EXTERNAL); // We are using an external analogue reference for the ADC
|
||||||
|
|
||||||
|
// TimerA0 initialization for control-loop interrupt.
|
||||||
|
|
||||||
|
TCA0.SINGLE.PER = 999; //
|
||||||
|
TCA0.SINGLE.CMP1 = 999; //
|
||||||
|
TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M.
|
||||||
|
TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm;
|
||||||
|
|
||||||
|
// TimerB0 initialization for PWM output
|
||||||
|
|
||||||
|
pinMode(6, OUTPUT);
|
||||||
|
TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz
|
||||||
|
analogWrite(6, 120);
|
||||||
|
|
||||||
|
interrupts(); //enable interrupts.
|
||||||
|
Wire.begin(); // We need this for the i2c comms for the current sensor
|
||||||
|
ina219.init(); // this initiates the current sensor
|
||||||
|
Wire.setClock(700000); // set the comms speed for i2c
|
||||||
|
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
|
||||||
|
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
|
||||||
|
Serial1.begin(9600); // Set up hardware UART
|
||||||
|
|
||||||
|
//Serial.println(getPWMfromSpeed(-1));
|
||||||
|
//Serial.println(getPWMfromSpeed(256));
|
||||||
|
//Serial.println(getPWMfromSpeed(0.5));
|
||||||
|
// Other Drive setup stuff
|
||||||
|
/////////currentHeading = REQUEST HEADING HERE;
|
||||||
|
|
||||||
|
analogWrite(pwmr, 0);
|
||||||
|
analogWrite(pwml, 0);
|
||||||
|
//digitalWrite(DIRR, LOW);
|
||||||
|
//digitalWrite(DIRL, HIGH);
|
||||||
|
|
||||||
|
pinMode(PIN_SS, OUTPUT);
|
||||||
|
pinMode(PIN_MISO, INPUT);
|
||||||
|
pinMode(PIN_MOSI, OUTPUT);
|
||||||
|
pinMode(PIN_SCK, OUTPUT);
|
||||||
|
|
||||||
|
SPI.begin();
|
||||||
|
SPI.setClockDivider(SPI_CLOCK_DIV32);
|
||||||
|
SPI.setDataMode(SPI_MODE3);
|
||||||
|
SPI.setBitOrder(MSBFIRST);
|
||||||
|
|
||||||
|
if (mousecam_init() == -1)
|
||||||
|
{
|
||||||
|
Serial.println("Mouse cam failed to init");
|
||||||
|
while (1)
|
||||||
|
;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int commandCompletionStatus = 0; //0-No Command, 1-New Command, 2-Command being run, 3-Command Complete
|
||||||
|
int requiredHeading = 0;
|
||||||
|
int distance = 0;
|
||||||
|
float spd = 0;
|
||||||
|
int currentHeading = 0;
|
||||||
|
//reset variables for update on completion
|
||||||
|
|
||||||
|
unsigned long previousMillis_Command = 0;
|
||||||
|
const long interval_Command = 1000;
|
||||||
|
DeserializationError error;
|
||||||
|
|
||||||
|
char asciiart(int k)
|
||||||
|
{
|
||||||
|
static char foo[] = "WX86*3I>!;~:,`. ";
|
||||||
|
return foo[k >> 4];
|
||||||
|
}
|
||||||
|
|
||||||
|
byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y];
|
||||||
|
DynamicJsonDocument rdoc(1024);
|
||||||
|
|
||||||
|
void loop()
|
||||||
|
{
|
||||||
|
if (Serial1.available() && (commandCompletionStatus == 0)){
|
||||||
|
// receive doc, not sure how big this needs to be
|
||||||
|
error = deserializeJson(rdoc, Serial1);
|
||||||
|
|
||||||
|
Serial.println("Got serial");
|
||||||
|
|
||||||
|
// Test if parsing succeeds.
|
||||||
|
if (error)
|
||||||
|
{
|
||||||
|
//Serial.print(F("deserializeJson() failed: "));
|
||||||
|
//Serial.println(error.f_str());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
//parsing success, prepare command and pull request information
|
||||||
|
commandCompletionStatus = 1;
|
||||||
|
requiredHeading = rdoc["rH"];
|
||||||
|
distance = rdoc["dist"];
|
||||||
|
spd = rdoc["sp"];
|
||||||
|
currentHeading = rdoc["cH"];
|
||||||
|
|
||||||
|
Serial.println("rH = " + String(requiredHeading) + " dist = " + String(distance) + " speed = " + String(spd));
|
||||||
|
|
||||||
|
//reset variables for update on completion
|
||||||
|
commandComplete = 0;
|
||||||
|
powerUsage_mWh = 0.0;
|
||||||
|
distTravelled_mm = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//if(current_x!=goal_x)
|
||||||
|
|
||||||
|
// Do Drive stuff, set the 5 values above
|
||||||
|
|
||||||
|
if (commandCompletionStatus == 0)
|
||||||
|
{ //noCommand
|
||||||
|
//Do Nothing just wait
|
||||||
|
//Serial.println("status0");
|
||||||
|
}
|
||||||
|
if (commandCompletionStatus == 1)
|
||||||
|
{ Serial.println("status1");
|
||||||
|
//newCommand
|
||||||
|
//set goals
|
||||||
|
goal_x += distance * sin(requiredHeading);
|
||||||
|
goal_y += distance * cos(requiredHeading);
|
||||||
|
total_y = 0;
|
||||||
|
total_x = 0;
|
||||||
|
commandCompletionStatus = 2;
|
||||||
|
|
||||||
|
initialAngleSet = false;
|
||||||
|
}
|
||||||
|
if (commandCompletionStatus == 2)
|
||||||
|
{ //Serial.println("status2");
|
||||||
|
//ongoingCommand
|
||||||
|
//start moving towards goal
|
||||||
|
|
||||||
|
//set angle first
|
||||||
|
if (!initialAngleSet)
|
||||||
|
{
|
||||||
|
//turn to angle
|
||||||
|
currentHeading = getCurrentHeading();
|
||||||
|
if (currentHeading < requiredHeading)
|
||||||
|
{ //turn right
|
||||||
|
Serial.println("turning right");
|
||||||
|
analogWrite(pwmr, getPWMfromSpeed(spd, false));
|
||||||
|
analogWrite(pwml, getPWMfromSpeed(spd, true));
|
||||||
|
digitalWrite(DIRR, LOW);
|
||||||
|
digitalWrite(DIRL, LOW);
|
||||||
|
}
|
||||||
|
else if (currentHeading > requiredHeading)
|
||||||
|
{ //turn left
|
||||||
|
Serial.println("turning left");
|
||||||
|
analogWrite(pwmr, getPWMfromSpeed(spd, false));
|
||||||
|
analogWrite(pwml, getPWMfromSpeed(spd, true));
|
||||||
|
digitalWrite(DIRR, HIGH);
|
||||||
|
digitalWrite(DIRL, HIGH);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
//heading correct therefore move to next step...
|
||||||
|
//STOP!!!!
|
||||||
|
digitalWrite(pwmr, LOW);
|
||||||
|
digitalWrite(pwml, LOW);
|
||||||
|
|
||||||
|
initialAngleSet = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{ //then move forwards but check angle for drift
|
||||||
|
if (total_x - distance < 0)
|
||||||
|
{ //go forwards
|
||||||
|
Serial.println("going forwards");
|
||||||
|
analogWrite(pwmr, getPWMfromSpeed(spd, false));
|
||||||
|
analogWrite(pwml, getPWMfromSpeed(spd, true));
|
||||||
|
digitalWrite(DIRR, LOW);
|
||||||
|
digitalWrite(DIRL, HIGH);
|
||||||
|
}
|
||||||
|
else if (total_x - distance > 0)
|
||||||
|
{ //go backwards
|
||||||
|
Serial.println("going backwards");
|
||||||
|
analogWrite(pwmr, getPWMfromSpeed(spd, false));
|
||||||
|
analogWrite(pwml, getPWMfromSpeed(spd, true));
|
||||||
|
digitalWrite(DIRR, HIGH);
|
||||||
|
digitalWrite(DIRL, LOW);
|
||||||
|
}
|
||||||
|
else if ((total_x == distance))
|
||||||
|
{ //distance met
|
||||||
|
//STOP!!!!!
|
||||||
|
digitalWrite(pwmr, LOW);
|
||||||
|
digitalWrite(pwml, LOW);
|
||||||
|
commandCompletionStatus = 3;
|
||||||
|
initialAngleSet = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (commandCompletionStatus == 3)
|
||||||
|
{ Serial.println("status3");
|
||||||
|
//currentPosMatchesOrExceedsRequest
|
||||||
|
///finish moving
|
||||||
|
|
||||||
|
//send update via UART
|
||||||
|
|
||||||
|
//prepare feedback variables
|
||||||
|
commandComplete = true;
|
||||||
|
current_x = goal_x;
|
||||||
|
current_y = goal_y;
|
||||||
|
distTravelled_mm += distance;
|
||||||
|
|
||||||
|
//compile energy use
|
||||||
|
unsigned long currentMillis_Energy = millis();
|
||||||
|
|
||||||
|
totalEnergyUsed += (currentMillis_Energy - previousMillis_Energy) * (powerUsed / loopCount) / 1000 / (60 * 60);
|
||||||
|
previousMillis_Energy = currentMillis_Energy;
|
||||||
|
|
||||||
|
if (debug){
|
||||||
|
Serial.print(motorVoltage);
|
||||||
|
Serial.print("Energy Used: ");
|
||||||
|
Serial.print(totalEnergyUsed);
|
||||||
|
Serial.println("mWh");
|
||||||
|
}
|
||||||
|
|
||||||
|
loopCount = 0; //reset counter to zero
|
||||||
|
powerUsed = 0; //reset power usage
|
||||||
|
powerUsage_mWh = totalEnergyUsed;
|
||||||
|
totalEnergyUsed = 0;
|
||||||
|
total_x1 = 0;
|
||||||
|
total_y1 = 0;
|
||||||
|
|
||||||
|
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
|
||||||
|
tdoc["comp"] = commandComplete;
|
||||||
|
tdoc["mWh"] = powerUsage_mWh;
|
||||||
|
tdoc["mm"] = distTravelled_mm;
|
||||||
|
tdoc["pos"][0] = current_x;
|
||||||
|
tdoc["pos"][1] = current_y;
|
||||||
|
serializeJson(tdoc, Serial); // Build JSON and send on UART1
|
||||||
|
commandCompletionStatus = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
//Handle power usage
|
||||||
|
//find motor voltage
|
||||||
|
//int motorVSensor = analogRead(A5);
|
||||||
|
//float motorVoltage = motorVSensor * (5.0 / 1023.0);
|
||||||
|
float motorVoltage = 5;
|
||||||
|
|
||||||
|
//find average power
|
||||||
|
|
||||||
|
if (current_mA >= 0){
|
||||||
|
powerUsed += current_mA * motorVoltage;
|
||||||
|
}
|
||||||
|
if (debug){
|
||||||
|
Serial.println(powerUsed);
|
||||||
|
}
|
||||||
|
|
||||||
|
//calculate averages for energy use calculations
|
||||||
|
loopCount += 1; //handle loop quantity for averaging
|
||||||
|
|
||||||
|
//find average current
|
||||||
|
//find average voltage
|
||||||
|
|
||||||
|
//update command/control
|
||||||
|
|
||||||
|
if (movementflag){
|
||||||
|
|
||||||
|
tdistance = tdistance + convTwosComp(xydat[0]);
|
||||||
|
// Serial.println("Distance = " + String(tdistance));
|
||||||
|
movementflag = 0;
|
||||||
|
delay(3);
|
||||||
|
}
|
||||||
|
int val = mousecam_read_reg(ADNS3080_PIXEL_SUM);
|
||||||
|
MD md;
|
||||||
|
mousecam_read_motion(&md);
|
||||||
|
/* for (int i = 0; i < md.squal / 4; i++)
|
||||||
|
Serial.print('*');
|
||||||
|
Serial.print(' ');
|
||||||
|
Serial.print((val * 100) / 351);
|
||||||
|
Serial.print(' ');
|
||||||
|
Serial.print(md.shutter);
|
||||||
|
Serial.print(" (");
|
||||||
|
Serial.print((int)md.dx);
|
||||||
|
Serial.print(',');
|
||||||
|
Serial.print((int)md.dy);
|
||||||
|
Serial.println(')'); */
|
||||||
|
|
||||||
|
// Serial.println(md.max_pix);
|
||||||
|
delay(100);
|
||||||
|
|
||||||
|
distance_x = md.dx; //convTwosComp(md.dx);
|
||||||
|
distance_y = md.dy; //convTwosComp(md.dy);
|
||||||
|
|
||||||
|
total_x1 = (total_x1 + distance_x);
|
||||||
|
total_y1 = (total_y1 + distance_y);
|
||||||
|
|
||||||
|
total_y = 10 * total_x1 / 157; //Conversion from counts per inch to mm (400 counts per inch)
|
||||||
|
total_x = 10 * total_y1 / 157; //Conversion from counts per inch to mm (400 counts per inch)
|
||||||
|
|
||||||
|
/* Serial.print('\n');
|
||||||
|
|
||||||
|
Serial.println("Distance_x = " + String(total_x));
|
||||||
|
|
||||||
|
Serial.println("Distance_y = " + String(total_y));
|
||||||
|
Serial.print('\n'); */
|
||||||
|
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
|
||||||
|
unsigned long currentMillis = millis();
|
||||||
|
if (loopTrigger)
|
||||||
|
{ // This loop is triggered, it wont run unless there is an interrupt
|
||||||
|
|
||||||
|
digitalWrite(13, HIGH); // set pin 13. Pin13 shows the time consumed by each control cycle. It's used for debugging.
|
||||||
|
|
||||||
|
// Sample all of the measurements and check which control mode we are in
|
||||||
|
sampling();
|
||||||
|
CL_mode = digitalRead(3); // input from the OL_CL switch
|
||||||
|
Boost_mode = digitalRead(2); // input from the Buck_Boost switch
|
||||||
|
|
||||||
|
if (Boost_mode)
|
||||||
|
{
|
||||||
|
if (CL_mode)
|
||||||
|
{ //Closed Loop Boost
|
||||||
|
pwm_modulate(1); // This disables the Boost as we are not using this mode
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{ // Open Loop Boost
|
||||||
|
pwm_modulate(1); // This disables the Boost as we are not using this mode
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if (CL_mode)
|
||||||
|
{ // Closed Loop Buck
|
||||||
|
// The closed loop path has a voltage controller cascaded with a current controller. The voltage controller
|
||||||
|
// creates a current demand based upon the voltage error. This demand is saturated to give current limiting.
|
||||||
|
// The current loop then gives a duty cycle demand based upon the error between demanded current and measured
|
||||||
|
// current
|
||||||
|
current_limit = 3; // Buck has a higher current limit
|
||||||
|
ev = vref - vb; //voltage error at this time
|
||||||
|
cv = pidv(ev); //voltage pid
|
||||||
|
cv = saturation(cv, current_limit, 0); //current demand saturation
|
||||||
|
ei = cv - iL; //current error
|
||||||
|
closed_loop = pidi(ei); //current pid
|
||||||
|
closed_loop = saturation(closed_loop, 0.99, 0.01); //duty_cycle saturation
|
||||||
|
pwm_modulate(closed_loop); //pwm modulation
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{ // Open Loop Buck
|
||||||
|
current_limit = 3; // Buck has a higher current limit
|
||||||
|
oc = iL - current_limit; // Calculate the difference between current measurement and current limit
|
||||||
|
if (oc > 0)
|
||||||
|
{
|
||||||
|
open_loop = open_loop - 0.001; // We are above the current limit so less duty cycle
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
open_loop = open_loop + 0.001; // We are below the current limit so more duty cycle
|
||||||
|
}
|
||||||
|
open_loop = saturation(open_loop, dutyref, 0.02); // saturate the duty cycle at the reference or a min of 0.01
|
||||||
|
pwm_modulate(open_loop); // and send it out
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// closed loop control path
|
||||||
|
|
||||||
|
digitalWrite(13, LOW); // reset pin13.
|
||||||
|
loopTrigger = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int getCurrentHeading(){
|
||||||
|
int currentAngle = 0; //-------------___<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
|
||||||
|
return currentAngle;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Timer A CMP1 interrupt. Every 800us the program enters this interrupt.
|
||||||
|
// This, clears the incoming interrupt flag and triggers the main loop.
|
||||||
|
|
||||||
|
ISR(TCA0_CMP1_vect){
|
||||||
|
TCA0.SINGLE.INTFLAGS |= TCA_SINGLE_CMP1_bm; //clear interrupt flag
|
||||||
|
loopTrigger = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// This subroutine processes all of the analogue samples, creating the required values for the main loop
|
||||||
|
|
||||||
|
void sampling(){
|
||||||
|
|
||||||
|
// Make the initial sampling operations for the circuit measurements
|
||||||
|
|
||||||
|
sensorValue0 = analogRead(A0); //sample Vb
|
||||||
|
sensorValue2 = analogRead(A2); //sample Vref
|
||||||
|
sensorValue3 = analogRead(A3); //sample Vpd
|
||||||
|
current_mA = ina219.getCurrent_mA(); // sample the inductor current (via the sensor chip)
|
||||||
|
|
||||||
|
// Process the values so they are a bit more usable/readable
|
||||||
|
// The analogRead process gives a value between 0 and 1023
|
||||||
|
// representing a voltage between 0 and the analogue reference which is 4.096V
|
||||||
|
|
||||||
|
vb = sensorValue0 * (4.096 / 1023.0); // Convert the Vb sensor reading to volts
|
||||||
|
vref = sensorValue2 * (4.096 / 1023.0); // Convert the Vref sensor reading to volts
|
||||||
|
vpd = sensorValue3 * (4.096 / 1023.0); // Convert the Vpd sensor reading to volts
|
||||||
|
|
||||||
|
// The inductor current is in mA from the sensor so we need to convert to amps.
|
||||||
|
// We want to treat it as an input current in the Boost, so its also inverted
|
||||||
|
// For open loop control the duty cycle reference is calculated from the sensor
|
||||||
|
// differently from the Vref, this time scaled between zero and 1.
|
||||||
|
// The boost duty cycle needs to be saturated with a 0.33 minimum to prevent high output voltages
|
||||||
|
|
||||||
|
if (Boost_mode == 1){
|
||||||
|
iL = -current_mA / 1000.0;
|
||||||
|
dutyref = saturation(sensorValue2 * (1.0 / 1023.0), 0.99, 0.33);
|
||||||
|
}else{
|
||||||
|
iL = current_mA / 1000.0;
|
||||||
|
dutyref = sensorValue2 * (1.0 / 1023.0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float saturation(float sat_input, float uplim, float lowlim){ // Saturation function
|
||||||
|
if (sat_input > uplim)
|
||||||
|
sat_input = uplim;
|
||||||
|
else if (sat_input < lowlim)
|
||||||
|
sat_input = lowlim;
|
||||||
|
else
|
||||||
|
;
|
||||||
|
return sat_input;
|
||||||
|
}
|
||||||
|
|
||||||
|
void pwm_modulate(float pwm_input){ // PWM function
|
||||||
|
analogWrite(6, (int)(255 - pwm_input * 255));
|
||||||
|
}
|
||||||
|
|
||||||
|
// This is a PID controller for the voltage
|
||||||
|
|
||||||
|
float pidv(float pid_input){
|
||||||
|
float e_integration;
|
||||||
|
e0v = pid_input;
|
||||||
|
e_integration = e0v;
|
||||||
|
|
||||||
|
//anti-windup, if last-time pid output reaches the limitation, this time there won't be any intergrations.
|
||||||
|
if (u1v >= uv_max){
|
||||||
|
e_integration = 0;
|
||||||
|
}else if (u1v <= uv_min){
|
||||||
|
e_integration = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
delta_uv = kpv * (e0v - e1v) + kiv * Ts * e_integration + kdv / Ts * (e0v - 2 * e1v + e2v); //incremental PID programming avoids integrations.there is another PID program called positional PID.
|
||||||
|
u0v = u1v + delta_uv; //this time's control output
|
||||||
|
|
||||||
|
//output limitation
|
||||||
|
saturation(u0v, uv_max, uv_min);
|
||||||
|
|
||||||
|
u1v = u0v; //update last time's control output
|
||||||
|
e2v = e1v; //update last last time's error
|
||||||
|
e1v = e0v; // update last time's error
|
||||||
|
return u0v;
|
||||||
|
}
|
||||||
|
|
||||||
|
// This is a PID controller for the current
|
||||||
|
|
||||||
|
float pidi(float pid_input){
|
||||||
|
float e_integration;
|
||||||
|
e0i = pid_input;
|
||||||
|
e_integration = e0i;
|
||||||
|
|
||||||
|
//anti-windup
|
||||||
|
if (u1i >= ui_max){
|
||||||
|
e_integration = 0;
|
||||||
|
}
|
||||||
|
else if (u1i <= ui_min){
|
||||||
|
e_integration = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
delta_ui = kpi * (e0i - e1i) + kii * Ts * e_integration + kdi / Ts * (e0i - 2 * e1i + e2i); //incremental PID programming avoids integrations.
|
||||||
|
u0i = u1i + delta_ui; //this time's control output
|
||||||
|
|
||||||
|
//output limitation
|
||||||
|
saturation(u0i, ui_max, ui_min);
|
||||||
|
|
||||||
|
u1i = u0i; //update last time's control output
|
||||||
|
e2i = e1i; //update last last time's error
|
||||||
|
e1i = e0i; // update last time's error
|
||||||
|
return u0i;
|
||||||
|
}
|
||||||
11
Drive/test/README
Normal file
11
Drive/test/README
Normal file
|
|
@ -0,0 +1,11 @@
|
||||||
|
|
||||||
|
This directory is intended for PlatformIO Unit Testing and project tests.
|
||||||
|
|
||||||
|
Unit Testing is a software testing method by which individual units of
|
||||||
|
source code, sets of one or more MCU program modules together with associated
|
||||||
|
control data, usage procedures, and operating procedures, are tested to
|
||||||
|
determine whether they are fit for use. Unit testing finds problems early
|
||||||
|
in the development cycle.
|
||||||
|
|
||||||
|
More information about PlatformIO Unit Testing:
|
||||||
|
- https://docs.platformio.org/page/plus/unit-testing.html
|
||||||
Loading…
Reference in a new issue