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

Merge pull request #2 from supleed2/Integration

Browse source 
Integration
This commit is contained in:
Aadi Desai 2021-06-14 12:18:27 +01:00 committed by GitHub
parent ddea9f26ca 54d30741e0
commit c3550ab4e1
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
10 changed files with 1011 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
Control/platformio.ini
View file

@ -13,11 +13,11 @@ platform = espressif32
board = esp32dev
framework = arduino
monitor_speed = 115200
upload_port = COM[3]
monitor_filters =
upload_port = COM13
monitor_filters =
send_on_enter
esp32_exception_decoder
build_flags =
build_flags =
-DCORE_DEBUG_LEVEL=5
-Wno-unknown-pragmas
build_type = debug

8
Control/ref/drive.cpp
View file

@ -19,12 +19,12 @@ void loop()
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 distance = rdoc["dist"]; // -1 for emergency stop
float speed = rdoc["sp"]; // -1 for emergency stop
float spd = rdoc["sp"]; // -1 for emergency stop
int currentHeading = rdoc["cH"];
bool resetDistanceTravelled = rdoc["rstD"];
bool resetDistanceTravelled = rdoc["rstD"];
bool commandComplete = 0;
float powerUsage_mW = 0.0;
float powerUsage_mWh = 0.0;
int distTravelled_mm = 0;
int current_x = 0;
int current_y = 0;
@ -33,7 +33,7 @@ void loop()
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["mW"] = powerUsage_mW;
tdoc["mWh"] = powerUsage_mWh;
tdoc["mm"] = distTravelled_mm;
tdoc["pos"][0] = current_x;
tdoc["pos"][1] = current_y;

3
Control/src/main.cpp
View file

@ -1,6 +1,7 @@
#pragma region Includes
#include <Arduino.h>
#include <string>
#include <SoftwareSerial.h> // Software Serial not currently needed
#include <SoftwareSerial.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
@ -305,7 +306,7 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
instr.instr = INSTR_CHARGE;
instr.charge = rdoc["rC"];
// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"]
queueInstruction(instr); // Put charge command in InstrQueue
}
break;

5
Drive/.gitignore vendored Normal file
View 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
View 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"
]
}

39
Drive/include/README Normal file
View 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
View 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
View 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

873
Drive/src/main.cpp Normal file
View file

@ -0,0 +1,873 @@
#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;
int dist_to_move_prev_fl = 0;
int dist_to_move_prev_fr = 0;
unsigned long time_pid_prev_fl = 0;
unsigned long time_pid_prev_fr = 0;
int dist_to_move_prev_sl = 0;
int dist_to_move_prev_sr = 0;
unsigned long time_pid_prev_sl = 0;
unsigned long time_pid_prev_sr = 0;
float kpdrive = 0.055;
float kddrive = 4.700;
float kpheading = 0.055;
float kdheading = 4.700;
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();
float pid_ms(int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds);
float pid_h_ms(bool left, float speed, int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds);
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 using optical flow
if (total_y - distance < 0)
{ //go forwards
//Serial.println("going forwards");
float speed_r = pid_ms(abs(total_y - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive);
float speed_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive);
float speed_r_head = pid_h_ms(0, speed_r, -total_x, &dist_to_move_prev_sr, &time_pid_prev_sr, kpheading, kdheading);
float speed_l_head = pid_h_ms(1, speed_l, -total_x, &dist_to_move_prev_sl, &time_pid_prev_sl, kpheading, kdheading);
analogWrite(pwmr, getPWMfromSpeed(speed_r, false));
analogWrite(pwml, getPWMfromSpeed(speed_l, true));
digitalWrite(DIRR, LOW);
digitalWrite(DIRL, HIGH);
}
else if (total_y - distance > 0)
{ //go backwards
//Serial.println("going backwards");
float speed_r = pid_ms(abs(total_y - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive);
float speed_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive);
float speed_r_head = pid_h_ms(0, speed_r, -total_x, &dist_to_move_prev_sr, &time_pid_prev_sr, kpheading, kdheading);
float speed_l_head = pid_h_ms(1, speed_l, -total_x, &dist_to_move_prev_sl, &time_pid_prev_sl, kpheading, kdheading);
analogWrite(pwmr, getPWMfromSpeed(speed_r, false));
analogWrite(pwml, getPWMfromSpeed(speed_l, true));
digitalWrite(DIRR, HIGH);
digitalWrite(DIRL, LOW);
}
else if ((total_y == 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_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;
}
}
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;
}
// This is a P!ID contrller for motor speed
float pid_ms(int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds){
int T_diff = millis() - *time_pid_prev;
float speed = (kps * dist_to_move) + ((kds/T_diff) * (dist_to_move - *dist_to_move_prev));
*time_pid_prev = millis();
Serial.println(speed);
if (speed >= 1) speed = 1;
else if (speed <= 0.5) speed = 0.5;
*dist_to_move_prev = dist_to_move;
return speed;
}
// This is a P!ID contrller for heading using optical flow
float pid_h_ms(bool left, float speed, int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds){
int T_diff = millis() - *time_pid_prev;
float speed_aug;
if ((dist_to_move > 0 && !left )||( dist_to_move < 0 && left)){
float speed_aug_mult = (kps * dist_to_move) + ((kds/T_diff) * (dist_to_move - *dist_to_move_prev));
if (speed_aug_mult >= 2) speed_aug_mult = 2;
else if (speed_aug_mult <= 1) speed_aug_mult = 1;
speed_aug = speed / dist_to_move;
}else{
speed_aug = speed;
}
*time_pid_prev = millis();
if (speed_aug >= 1) speed_aug = 1;
else if (speed_aug <= 0.3) speed_aug = 0.3;
*dist_to_move_prev = dist_to_move;
return speed_aug;
}

11
Drive/test/README Normal file
View 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