yiwen zou
Coursework 2 Motor Control
Diff: main.cpp
- Revision:
- 11:f101c1f3d3bd
- Parent:
- 10:a4b5723b6c9d
- Child:
- 12:9b7a85e17146
--- a/main.cpp Tue Feb 26 12:23:17 2019 +0000
+++ b/main.cpp Fri Mar 22 23:43:49 2019 +0000
@@ -1,4 +1,6 @@
#include "mbed.h"
+#include "Crypto.h"
+#include "rtos.h"
//Photointerrupter input pins
#define I1pin D3
@@ -19,9 +21,154 @@
#define PWMpin D9
-//Motor current sense
-#define MCSPpin A1
-#define MCSNpin A0
+
+
+
+
+
+Thread motorCtrlT (osPriorityNormal, 1024);
+Thread OutputT (osPriorityNormal,1024);
+Thread DecodeT (osPriorityNormal,1024);
+RawSerial pc(SERIAL_TX, SERIAL_RX);
+
+//Global variables
+#define MAX_PWM 10000
+Queue<void,8> inCharQ;
+volatile uint64_t newKey;
+volatile float newRev = 0.001; //default zero rotation
+volatile float maxSpeed = 50; //1800 rotations per second
+volatile float motorPosition;
+volatile float originalmotorPosition;
+volatile float pulseWidth = MAX_PWM;
+int32_t motorVelocity;
+volatile float kps = 30;
+volatile float kis = 0.75;
+volatile float kpr = 20;
+volatile float kdr = 8.5;
+
+
+//Protect variable from being accessed by other threads
+Mutex newKey_mutex;
+
+
+
+typedef struct{ //define structure of mail
+ uint8_t code;
+ float data;
+ uint64_t longData;
+ } mail_t;
+
+Mail<mail_t,16> outMail;
+
+
+void serialISR(){ //Rawserial Interrupts
+ uint8_t newChar = pc.getc();
+ inCharQ.put((void*)newChar);
+ }
+
+void putMessage(uint8_t code, float data){ //Mail for queueing messages
+ mail_t *pMail = outMail.alloc();
+ pMail -> code = code;
+ pMail -> data = data;
+ outMail.put(pMail);
+}
+
+// Overloaded version of putMessage for int versions of data
+void putMessage(uint8_t code, uint64_t data){
+ mail_t *pMail = outMail.alloc();
+ pMail -> code = code;
+ pMail -> longData = data;
+ outMail.put(pMail);
+}
+
+
+void Decode(){ //Decode User Input Command
+ char newCommand[50]; //Array used to hold commands
+ uint8_t index = 0;
+ pc.attach(&serialISR); //Attach rawserial
+ while(1) {
+ osEvent newEvent = inCharQ.get(); //New event created when new character detected
+ uint8_t newChar = (uint8_t)newEvent.value.p;
+ newCommand[index] = newChar;
+
+ if(index == 49) { //Checks whether buffer overflows
+ pc.printf("Buffer Overflow!\n\r");
+ index = 0;
+ }
+ else if(newChar == '\r'){ // \r indicates end of command, checks first character of command and reset buffer
+ newCommand[index] = '\0';
+ index = 0;
+ if (newCommand[0] == 'K'){ // K -> New Key , case 3
+ newKey_mutex.lock();
+ sscanf(newCommand, "K%x", &newKey);
+ putMessage(3,newKey);
+ newKey_mutex.unlock();
+ }
+ if(newCommand[0] == 'V'){
+ sscanf(newCommand,"V%f",&maxSpeed);
+ putMessage(6,maxSpeed);
+ if(maxSpeed > 20){ // if the target velocity is large enough
+ kps = 30;
+ kis = 0.75;
+ kpr = 20;
+ kdr = 8.5;
+ }
+ else{ // otherwise for small velocities change the parameters
+ kps = 20;
+ kis = 2;
+ kpr = 18;
+ kdr = 5;
+ }
+ }
+ if(newCommand[0] == 'R'){
+ sscanf(newCommand, "R%f", &newRev);
+ putMessage(7,newRev);
+ originalmotorPosition = motorPosition;
+ }
+ }
+ else { //Keep loading
+ index++;
+ }
+ }
+}
+
+//Serial Outputs
+void OutputMail(){
+ while (1) {
+ //New event created when user enters command
+ osEvent newEvent = outMail.get();
+ mail_t *pMail = (mail_t*)newEvent.value.p;
+
+ //Use switch to handle different cases, case defined by code
+ switch(pMail->code){
+ case 1:
+ pc.printf("Hash rate %.0f\n\r", pMail->data);
+ break;
+ case 2:
+ pc.printf("Hash computed at 0x%016x\n\r", pMail->longData);
+ break;
+ case 3:
+ pc.printf("Sequence key set to 0x%016llx\n\r", pMail->longData);
+ break;
+ case 4:
+ pc.printf("Motor Position is %.2f\n\r", pMail->data);
+ break;
+ case 5:
+ pc.printf("Motor Velocity is %.2f\n\r", pMail->data);
+ break;
+ case 6:
+ pc.printf("Max Speed now is set to %.2f\n\r", pMail->data);
+ break;
+ case 7:
+ pc.printf("Revolution now is set to %.2f\n\r", pMail->data);
+ break;
+ default:
+ pc.printf("Message %d with data 0x%016x\n\r", pMail->code, pMail->data);
+ }
+ //free memory location allocated
+ outMail.free(pMail);
+ }
+}
//Mapping from sequential drive states to motor phase outputs
/*
@@ -34,6 +181,7 @@
5 H L -
6 - - -
7 - - -
+
*/
//Drive state to output table
const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
@@ -43,15 +191,23 @@
//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
-const int8_t lead = 2; //2 for forwards, -2 for backwards
+int8_t lead = 2; //2 for forwards, -2 for backwards
+
+int8_t orState = 0; //Rotot offset at motor state 0
+
+
+void motorCtrlTick(){ //call back function that starts motor control
+ motorCtrlT.signal_set(0x1);
+}
+
//Status LED
DigitalOut led1(LED1);
//Photointerrupter inputs
-DigitalIn I1(I1pin);
-DigitalIn I2(I2pin);
-DigitalIn I3(I3pin);
+InterruptIn I1(I1pin);
+InterruptIn I2(I2pin);
+InterruptIn I3(I3pin);
//Motor Drive outputs
DigitalOut L1L(L1Lpin);
@@ -61,12 +217,18 @@
DigitalOut L3L(L3Lpin);
DigitalOut L3H(L3Hpin);
+//PwmOut Pin
+PwmOut PWMD9(PWMpin);
+
+
//Set a given drive state
-void motorOut(int8_t driveState){
+void motorOut(int8_t driveState, uint32_t motorTorque){
//Lookup the output byte from the drive state.
int8_t driveOut = driveTable[driveState & 0x07];
+
+
//Turn off first
if (~driveOut & 0x01) L1L = 0;
if (~driveOut & 0x02) L1H = 1;
@@ -82,46 +244,259 @@
if (driveOut & 0x08) L2H = 0;
if (driveOut & 0x10) L3L = 1;
if (driveOut & 0x20) L3H = 0;
+
+ //d9.write(motorTorque);
+ PWMD9.pulsewidth_us(motorTorque);
+
}
- //Convert photointerrupter inputs to a rotor state
+//Convert photointerrupter inputs to a rotor state
inline int8_t readRotorState(){
return stateMap[I1 + 2*I2 + 4*I3];
- }
+}
//Basic synchronisation routine
int8_t motorHome() {
- //Put the motor in drive state 0 and wait for it to stabilise
- motorOut(0);
+ //Put the motor in drive state 0 and wait for it to stabilize
+ motorOut(0, 700000);
+ //motorOut(0);
wait(2.0);
//Get the rotor state
return readRotorState();
}
+
+
+void motorISR(){
+ static int8_t oldrotorState;
+ int8_t rotorState = readRotorState();
+ //pc.printf("Im here");
+ motorOut((rotorState-orState+lead+6)%6,pulseWidth); //+6 to make sure the remainder is positive
+
+ if (rotorState - oldrotorState == 5) motorPosition --;
+ else if (rotorState - oldrotorState == -5) motorPosition ++;
+ else motorPosition += (rotorState - oldrotorState);
+ oldrotorState = rotorState;
+}
+
+/*
+void Torque(){
+
+}
+*/
+
+
+void motorCtrlFn(){
+ int32_t counter = 0;
+ static int32_t oldmotorPosition;
+ Timer t;
+ t.start();
+
+ //Define variables and parameters being used for motor control
+ float motorPos;
+ float Speed;
+ float Revolution;
+ float outputTorqueS;
+ float outputTorqueR;
+ float Torque;
+ float rateofchangeofPositionError;
+ float oldError;
+ float errorSum;
+ float speedError;
+ float positionError;
+ float c = 42.0;
+
+ //Default Lead
+ int8_t outputLeadS = 2;
+ int8_t outputLeadR = 2;
+
+ //Sign of direction
+ int8_t errorSign = 1;
+
+ //Define ticker to measure time interval
+ Ticker motorCtrlTicker;
+ motorCtrlTicker.attach_us(&motorCtrlTick,100000);
+
+ while(1){
+ motorCtrlT.signal_wait(0x1); //executes every 100ms
+
+ errorSum = 0;
+
+ motorPos = motorPosition;
+ Speed = maxSpeed*6;
+ Revolution = newRev*6;
+
+ //Calculate rate of change of position = velocity
+ motorVelocity = (motorPos - oldmotorPosition)/t.read();
+
+
+ //
+ if(motorVelocity >= 0) errorSign = 1;
+ else errorSign = -1;
+
+ //Calculate rate of change of position error = differential term
+ positionError = Revolution + originalmotorPosition - motorPos;
+ rateofchangeofPositionError = (positionError - oldError)/t.read();
+
+ //Calculate speed error
+ speedError = Speed - abs(motorVelocity);
+
+
+ oldmotorPosition = motorPos;
+
+ //Calculate output Torque for speed and position
+
+ if(Speed == 0) outputTorqueS = MAX_PWM;
+ else outputTorqueS = (kps*((Speed+c)- abs(motorVelocity))+kis*errorSum)*errorSign;
+
+ outputTorqueR = kpr*positionError + kdr*rateofchangeofPositionError;
+
+ //set upper limit for integral term
+ if(kis*errorSum <= MAX_PWM){
+ errorSum += speedError*t.read();
+ }
+
+ t.reset(); //ticker reset
+
+ //Determine output lead depending on sign of torque
+
+ if(outputTorqueR >= 0) {
+ outputLeadR = 2;
+ } else {
+ outputLeadR = -2;
+ }
+ if(Speed !=0 ){
+ if(outputTorqueS >= 0) {
+ outputLeadS = 2;
+ }
+ else {
+ outputLeadS = -2;
+ }
+ if(outputTorqueS > MAX_PWM) {
+ outputTorqueS = MAX_PWM;
+ }
+ if(outputTorqueS < -MAX_PWM) {
+ outputTorqueS = -MAX_PWM;
+ }
+ }
+ else {
+ outputTorqueS = MAX_PWM;
+ }
+
+
+ // pick the slower one to limit speed to maxSpeed
+ if(newRev == 0){
+ pulseWidth = abs(outputTorqueS);
+ }
+ else{
+ if(motorVelocity < 0){
+ if(outputTorqueS >= outputTorqueR){
+ pulseWidth = abs(outputTorqueS);
+ lead = outputLeadS;
+ }
+ else {
+ pulseWidth = abs(outputTorqueR);
+ lead = outputLeadR;
+ }
+ }
+ else {
+ if (outputTorqueS <= outputTorqueR){
+ pulseWidth = abs(outputTorqueS);
+ lead = outputLeadS;
+ }
+ else {
+ pulseWidth = abs(outputTorqueR);
+ lead = outputLeadR;
+ }
+ }
+ }
+
+ if(motorVelocity == 0) motorISR();
+
+
+ //Output position and velocity when counter counts to 10
+ counter++;
+ if(counter == 10){
+ counter = 0;
+ putMessage(4,(float)(motorPosition/6.0));
+ putMessage(5,(float)(motorVelocity/6.0));
+ }
+
+ //Redefine old position error
+ oldError = positionError;
+ }
+}
+
+
+float hashCount = 0;
+
+void calcHashRate(){
+ putMessage(1,hashCount);
+ hashCount = 0;
+}
+
+
+
//Main
int main() {
- int8_t orState = 0; //Rotot offset at motor state 0
- int8_t intState = 0;
- int8_t intStateOld = 0;
-
- //Initialise the serial port
- Serial pc(SERIAL_TX, SERIAL_RX);
- pc.printf("Hello\n\r");
-
- //Run the motor synchronisation
+ //set up pwm period
+ PWMD9.period(0.002f); // 2ms second period
+ PWMD9.write(1.0f); // 100% duty cycle, relative to period
orState = motorHome();
- pc.printf("Rotor origin: %x\n\r",orState);
- //orState is subtracted from future rotor state inputs to align rotor and motor states
+ pc.printf("Rotor origin: %x\n\r", orState);
+
+ motorCtrlT.start(motorCtrlFn);
+ OutputT.start(OutputMail);
+ DecodeT.start(Decode);
+
+ // Run the motor synchronisation
+ pc.printf("Rotor origin: %x\n\r", orState);
+
+ // motor controlling interrupt routines
+ I1.rise(&motorISR);
+ I1.fall(&motorISR);
+ I2.rise(&motorISR);
+ I2.fall(&motorISR);
+ I3.rise(&motorISR);
+ I3.fall(&motorISR);
- //Poll the rotor state and set the motor outputs accordingly to spin the motor
+ // mining bitcoins
+ SHA256 mine;
+ uint8_t sequence[] = {
+ 0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,
+ 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,
+ 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,
+ 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,
+ 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,
+ 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+
+
+ uint64_t* key = (uint64_t*)((int)sequence + 48);
+ uint64_t* nonce = (uint64_t*)((int)sequence + 56);
+ uint8_t hash[32];
+
+ // timer for hash rate
+ Ticker t;
+ t.attach(&calcHashRate, 1.0);
+
+
while (1) {
- intState = readRotorState();
- if (intState != intStateOld) {
- intStateOld = intState;
- motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
- //pc.printf("%d\n\r",intState);
+ //Protect key from other accesses
+ newKey_mutex.lock();
+ *key = newKey;
+ newKey_mutex.unlock();
+ //Computer Hash with correct conditions
+ mine.computeHash(hash, sequence, 64);
+ hashCount = hashCount + 1;
+ if (hash[0] == 0 && hash[1] == 0){
+ putMessage(2, *nonce);
+ //pc.printf("Key: 0x%016llx\n\r", *key);
}
+ *nonce = *nonce + 1;
+
}
}