George Padley
Final version
Dependencies: Crypto_light mbed-rtos mbed
Fork of
EMBEDDED_CW2_Final
by 
Jacob Kay
Diff: main.cpp
- Revision:
- 6:96383d87c51f
- Parent:
- 5:e4b799086bc1
--- a/main.cpp Fri Mar 23 15:42:42 2018 +0000
+++ b/main.cpp Fri Mar 23 21:01:44 2018 +0000
@@ -1,7 +1,6 @@
#include "mbed.h"
#include "SHA256.h"
#include "rtos.h"
-#include "slre.h"
#define char_len_max 32
//Photointerrupter input pins
@@ -21,6 +20,7 @@
#define L3Lpin D9 //0x10
#define L3Hpin D10 //0x20
+
//Mapping from sequential drive states to motor phase outputs
/*
State L1 L2 L3
@@ -38,7 +38,6 @@
//Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
-//const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
//Phase lead to make motor spin
int8_t lead = 2; //2 for forwards, -2 for backwards
@@ -72,13 +71,11 @@
NONCE = 2, //correct nonce ID
POSITION = 3, //Starting Rotor ID
DECODED = 4, //Decoded message ID
- NEW_KEY = 5,
- OLD_KEY = 6,
- VELOCITY = 7,
+ NEW_KEY = 5, //new key ID
+ OLD_KEY = 6, //old key ID
+ VELOCITY = 7, //velocity
NEW_SPEED = 8,
- OLD_SPEED = 9,
- NEW_TORQUE = 10,
- NEW_ROTATIONS = 11
+ NEW_ROTATIONS = 9
};
//message structure
@@ -105,15 +102,14 @@
Mutex newKey_mutex; //Stops the value from beng changed during use
float newSpeed = 30.0f;
Mutex newSpeed_mutex;
-uint32_t period = 2000;
uint32_t torqueVal = 1000;
-int32_t kp = 25;
-int32_t kp2 = 25;
-int32_t kd = 20; //Why aren't we using a define??
float noRotations = 0.0f;
bool dirSwitch = false;
bool rotate = false;
bool rotStart = false;
+int period = 2000;
+int kp = 25.0f;
+int kd = 20.0f;
Thread commOutT(osPriorityNormal,1024); //Output Thread
Thread commInT(osPriorityNormal,1200); //Input Thread
@@ -153,8 +149,6 @@
case ERROR_C:
if(pMessage->data == 0) { //Input message was too large
pc.printf("Input command too large\n\r");
- } else if(pMessage->data == 1) { //Input message was too large
- pc.printf("Key of wrong format\n\r");
}
break;
case HASH:
@@ -189,12 +183,6 @@
case NEW_SPEED:
pc.printf("New speed: %f\n\r",pMessage->dataf);
break;
- case OLD_SPEED:
- pc.printf("New speed same as old speed: %d\n\r",pMessage->data);
- break;
- case NEW_TORQUE:
- pc.printf("New torque: %d\n\r",pMessage->data);
- break;
case NEW_ROTATIONS:
pc.printf("New number of rotations: %f\n\r",pMessage->dataf);
break;
@@ -211,9 +199,6 @@
void decode_char(char* buffer, uint8_t index)
{
-
- struct slre regex;
- struct cap captures[0 + 1];
if(buffer[index] == 'V') { //if first value is R rotate cretain number of times
putMessage(DECODED,(uint64_t)0);
newSpeed_mutex.lock();
@@ -250,29 +235,18 @@
rotStart = true;
putMessage(NEW_ROTATIONS,noRotations);
} else if (buffer[index] == 'K') { //if char is K set key to value input
- putMessage(DECODED,(uint64_t)2);
- if(!slre_compile(®ex, "K[0-9a-fA-F]{16}")) {
- putMessage(ERROR_C,(uint64_t)1);
- } else if(slre_match(®ex, buffer, 16, captures)) {
- newKey_mutex.lock();
- sscanf(buffer, "K%llx", &newKey);
- if(oldKey != newKey) {
- putMessage(NEW_KEY,newKey);
- *key = newKey;
- oldKey = newKey;
- } else {
- putMessage(OLD_KEY,oldKey);
- }
- newKey_mutex.unlock();
+ newKey_mutex.lock();
+ sscanf(buffer, "K%llx", &newKey);
+ if(oldKey != newKey) {
+ putMessage(NEW_KEY,newKey);
+ *key = newKey;
+ oldKey = newKey;
} else {
- putMessage(ERROR_C,(uint64_t)1);
+ putMessage(OLD_KEY,oldKey);
}
+ newKey_mutex.unlock();
} else if (buffer[index] == 'k') { //if char is K set key to value input
putMessage(DECODED,(uint64_t)2);
-// if(!slre_compile(®ex, "k[0-9a-fA-F]{16}")){
-// putMessage(ERROR_C,1);
-// }
-// else if(slre_match(®ex, buffer, char_len_max, captures)){
newKey_mutex.lock();
sscanf(buffer, "k%llx", &newKey);
if(oldKey != newKey) {
@@ -283,13 +257,6 @@
putMessage(OLD_KEY,oldKey);
}
newKey_mutex.unlock();
-// }
-// else {
-// putMessage(ERROR_C,1);
-// }
- } else if (buffer[index] == 'p') { //if char is K set key to value inpu
- sscanf(buffer, "p%lld", &kp);
- putMessage(NEW_TORQUE,(uint64_t)kp);
}
}
@@ -298,10 +265,10 @@
pc.printf("Enter your command:\n\r"); //Tells the person to input their message
pc.attach(&serialISR); //looks for the serialISR to get message
while(1) {
+
if(ptr >= char_len_max) {
putMessage(ERROR_C,(uint64_t)0); //if gone over the buffer length, cancel and restart for next input
ptr = 0; //reset pointer
- break;
}
osEvent newEvent = inCharQ.get(); //get next character
uint8_t newChar = (uint8_t)newEvent.value.p;
@@ -314,6 +281,7 @@
ptr = 0; //resets the pointer
decode_char(commInChar,ptr); //sends array to decoding function
}
+
}
}
@@ -333,7 +301,7 @@
if (~driveOut & 0x20) L3H = 1;
//Then turn on
- if (driveOut & 0x01) L1L.pulsewidth_us(torque);
+ if (driveOut & 0x01) L1L.pulsewidth_us(torque); //motor torque output
if (driveOut & 0x02) L1H = 0;
if (driveOut & 0x04) L2L.pulsewidth_us(torque);
if (driveOut & 0x08) L2H = 0;
@@ -353,8 +321,8 @@
//Put the motor in drive state 0 and wait for it to stabilise
//motorOut(0,torqueVal);
motorOut(0,1000);
- wait(1.0);
- lead = 0;
+ wait(1.0); //waits for stabalisation
+ lead = 0; //stops rotation
//Get the rotor state
return readRotorState();
@@ -363,27 +331,27 @@
int32_t motorPosition;
void motorISR()
{
- static int8_t oldRotorState;
+ static int8_t oldRotorState; //remembers old state
int8_t rotorState = readRotorState(); //reads motor position
motorOut((rotorState-orState+lead+6)%6,torqueVal); //+6 to make sure the remainder is positive
- if(rotorState - oldRotorState==5) motorPosition--;
- else if(rotorState - oldRotorState== -5) motorPosition++;
+ if(rotorState - oldRotorState==5) motorPosition--; //reverse
+ else if(rotorState - oldRotorState== -5) motorPosition++; //forward
else motorPosition+=(rotorState - oldRotorState);
- oldRotorState = rotorState;
+ oldRotorState = rotorState;//remember previous state
}
void motorCtrlTick()
{
- motorCtrlT.signal_set(0x1);
+ motorCtrlT.signal_set(0x1); //wait for 100 ms to send signal
}
Timer t_motor;
void motorCtrlFn()
{
- float v, v_avg, ys, yr, dEr;
+ float v, v_avg, ys, yr, dEr; //local variables used
int i = 0, dt, oldPosition, totPosition, position, startPosition, newEr, oldEr, mainLead;
- bool jacobFudge = false;
+ bool jumpStart = false;
t_motor.start();
Ticker motorCtrlTicker;
motorCtrlTicker.attach_us(&motorCtrlTick,100000);
@@ -392,19 +360,19 @@
if(rotate) {
if(rotStart) {
if(noRotations > 0) {
- lead = 2;
+ lead = 2;//positive rotations
} else if(noRotations < 0) {
- lead = -2;
+ lead = -2;//negative rotations
} else if(noRotations == 0 && lead == 0) {
- lead = 2;
+ lead = 2;//if no direction specified spin forwards
}
- i = 0;
+ i = 0; //reset variables
v_avg = 0;
yr = 0.0f;
ys = 0.0f;
dEr = 0.0f;
mainLead = lead; //sets general direction
- totPosition = (int)6*noRotations; //nimber of position changes required
+ totPosition = (int)6*noRotations; //number of position changes required
oldEr = totPosition; //how far away
rotStart = false; //stops from running this loop
__disable_irq(); //disables interrupts
@@ -414,7 +382,7 @@
motorPosition = 0; //resets time and motorPosition
__enable_irq(); //enables interrupts
position = 0;
- jacobFudge = true;
+ jumpStart = true;
} else if(noRotations == 0) { //if to spin forever
i++; //increment counter
@@ -435,14 +403,14 @@
newSpeed_mutex.unlock();
ys = kp*(newSpeed-abs(v)); //speed controller
if(ys < 0) {
- lead = mainLead*-1;
+ lead = mainLead*-1; //reverses direction
} else {
- lead = mainLead;
+ lead = mainLead; //goes forwards
}
if(abs(ys) > 1000) {
- torqueVal = 1000;
+ torqueVal = 1000; //maximum speed
} else {
- torqueVal = abs(ys);
+ torqueVal = abs(ys); //makes sure absolute
}
// pc.printf("torque = %d\r\n",torqueVal);
} else {
@@ -461,61 +429,60 @@
newEr = totPosition-position; //difference in placement
dEr = 1000.0f*((float)newEr-(float)oldEr)/(float)dt; //change against time
oldEr = newEr; //old is same as new
- yr = (float)kp2*(float)newEr + (float)kd*dEr; //rotational controller
+ yr = (float)kp*(float)newEr + (float)kd*dEr; //rotational controller
v_avg += v; //adds speed onto averager
ys = (float)kp*((float)newSpeed-fabsf(v))*((newEr > 0) ? 1.0f : ((newEr < 0) ? -1.0f : 0.0f)); //speed controller
- if(jacobFudge == true) {
+ if(jumpStart == true) {
lead = mainLead; //makes sure it's in the correct direction
torqueVal = 900; //sets torque
motorISR(); //moves the motor
- jacobFudge = false;
+ jumpStart = false;
}
if(v >=0.0f) { //if speed is +ve
if(abs(ys)<abs(yr)) {
- torqueVal = abs(ys);
+ torqueVal = abs(ys); //sets the torque output
} else {
- torqueVal = abs(yr);
+ torqueVal = abs(yr);//sets the torque output
}
- if(abs(newEr) <=5) {
+ if(abs(newEr) <=5) {//stops it spinning
torqueVal = 0;
lead = 0;
rotate = false;
pc.printf("NewErr %d\r\n",newEr);
- } else if (yr<-500 && dEr <0)
- torqueVal = abs(yr);
+ } else if (yr<-500 && dEr <0) {
+ torqueVal = abs(yr);//go backwards
lead = -2;
} else {
- lead = 2;
+ lead = 2; //go forwards
}
} else {
if(abs(ys)>abs(yr)) {
- torqueVal = abs(ys);
+ torqueVal = abs(ys); //take the highest value for torque
} else {
torqueVal = abs(yr);
}
- if(abs(newEr) <=5) {
+ if(abs(newEr) <=5) { //stop spinning when error loess than 1 rotation
torqueVal = 0;
lead = 0;
rotate = false;
- pc.printf("NewErr %d\r\n",newEr);
- } else if (yr>500 && dEr >0)
+ } else if (yr>500 && dEr >0) { //goes backwards if overshot
torqueVal = abs(yr);
lead = 2;
} else {
lead = -2;
}
}
- if(torqueVal != 0 && lead !=0 && abs(v)== 0) {
+ if(torqueVal != 0 && lead !=0 && abs(v)== 0) { //makes sure motor spins if it is meant to
torqueVal = torqueVal + 50;
motorISR();
}
- if(torqueVal > 1000) {
+ if(torqueVal > 1000) { //stops non linear torque
torqueVal = 1000;
}
}
}
- if (i==10) {
+ if (i==10) {//every 1 second print velocity
v_avg = v_avg/i;
putMessage(VELOCITY, v_avg);
v_avg = 0;