Olaf Sikorski
This is probably never gonna get done
Diff: main.cpp
- Revision:
- 20:5bd9f9e406d1
- Parent:
- 18:e48c0910c71e
- Child:
- 21:34f440ae0227
diff -r e48c0910c71e -r 5bd9f9e406d1 main.cpp
--- a/main.cpp Mon Mar 18 16:30:12 2019 +0000
+++ b/main.cpp Tue Mar 19 22:53:22 2019 +0000
@@ -44,7 +44,7 @@
//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
+const int8_t lead = -2; //2 for forwards, -2 for backwards
//Status LED
DigitalOut led1(LED1);
@@ -55,30 +55,34 @@
InterruptIn I3(I3pin);
//Motor Drive outputs
-PwmOut L1L(L1Lpin);
+DigitalOut L1L(L1Lpin);
DigitalOut L1H(L1Hpin);
-PwmOut L2L(L2Lpin);
+DigitalOut L2L(L2Lpin);
DigitalOut L2H(L2Hpin);
-PwmOut L3L(L3Lpin);
+DigitalOut L3L(L3Lpin);
DigitalOut L3H(L3Hpin);
-
+PwmOut d9 (PWMpin);
int8_t orState = 0;
-int8_t intState = 0;
-int8_t intStateOld = 0;
+//int8_t intState = 0;
+//int8_t intStateOld = 0;
int32_t revoCounter = 0; //Counts the number of revolutions
int32_t motorVelocity;
//Phase lead to make motor spin
-int8_t lead = -2; //2 for forwards, -2 for backwards
+//int8_t lead = -2; //2 for forwards, -2 for backwards
typedef struct {
- uint64_t nonce;
- float data;
+ int message;
+ uint64_t data;
+ float fdata;
} mail_t;
-
+
Mail<mail_t, 16> mail_box;
-Thread commandProcessorthread;
-Thread bitcointhread;
+Thread commandProcessorthread(osPriorityNormal,1024);
+Thread bitcointhread(osPriorityNormal,1024);
+Thread motorCtrlT(osPriorityNormal,1024);
+Thread commsOut(osPriorityNormal,1024);
+
RawSerial pc(SERIAL_TX, SERIAL_RX);
Queue<void, 8> inCharQ;
Mutex newKey_mutex;
@@ -90,17 +94,200 @@
float motorPosition_command;
float motorPosition;
-// mail to queue messages for serial port
-void putMessage(uint64_t *nonce,float data){
+void serialISR() {
+ uint8_t newChar = pc.getc();
+ inCharQ.put((void*) newChar);
+}
+/***************************************************************/
+//The following block should be moved to a library, but I don't the time to figure this out atm.
+
+//CommsOut.h
+
+enum message_keys {
+KEY_DECLINED = 0,
+ROTATION_CMD = 1,
+MAX_SPEED_CMD = 2,
+KEY_ECHO = 3,
+R_ECHO1 = 4,
+R_ECHO2 = 5,
+MAX_SPEED_ECHO1= 6,
+MAX_SPEED_ECHO2= 7,
+
+
+INVALID_CMD = 10,
+MOTOR_POS = 11,
+MOTOR_SPEED = 12,
+
+
+NONCE_DETECT = 14,
+ROTOR_ORG = 15,
+
+WELCOME = 20
+};
+
+void putMessage(int message, uint64_t data = 0, float fdata=0){
mail_t *mail = mail_box.alloc();
- mail->nonce = *nonce;
- mail->data = *data;
+ mail->message = message;
+ mail->data = data;
+ mail->fdata = fdata;
mail_box.put(mail);
}
-void serialISR() {
- uint8_t newChar = pc.getc();
- inCharQ.put((void*) newChar);
+void commsOutFunc() {
+ while (true) {
+ osEvent evt = mail_box.get();
+ if (evt.status == osEventMail) {
+ mail_t *mail = (mail_t*)evt.value.p;
+ switch (mail->message) {
+ case KEY_DECLINED :
+ pc.printf("Not a valid key!\n\r");
+ break;
+ case ROTATION_CMD :
+ pc.printf("Rotation command\n\r");
+ break;
+ case MAX_SPEED_CMD :
+ pc.printf("Max speed command\n\r");
+ break;
+ case R_ECHO1 :
+ pc.printf("Rotor command:\n\r");
+ pc.printf("Full rotations: %d\n\r", mail->data);
+ break;
+ case R_ECHO2 :
+ pc.printf("Partial rotation: %d\n\r", mail->data);
+ break;
+ case MAX_SPEED_ECHO1 :
+ pc.printf("Max speed command:\n\r");
+ pc.printf("Max speed int: %d\n\r", mail->data);
+ break;
+ case MAX_SPEED_ECHO2 :
+ pc.printf("Max speed decimal: %d\n\r", mail->data);
+ break;
+ case KEY_ECHO :
+ pc.printf("Received key: %016llx\n\r", mail->data);
+ break;
+ case INVALID_CMD :
+ pc.printf("Invalid command!\r\n");
+ break;
+ case MOTOR_POS :
+ pc.printf("Motor position: %f\r\n", mail->fdata);
+ //pc.printf("{TIMEPLOT|%.2f|Motor Position", mail->fdata);
+ break;
+ case MOTOR_SPEED :
+ pc.printf("Motor speed: %f\r\n", mail->fdata);
+ break;
+ case NONCE_DETECT :
+ pc.printf("Successful nonce: %016x\n\r", mail->data);
+ break;
+ case WELCOME :
+ pc.printf("Hello Pete\n\r");
+ break;
+ case ROTOR_ORG :
+ pc.printf("Rotor origin: %x\n\r", orState);
+
+ }
+ mail_box.free(mail);
+ }
+ }
+}
+//End of that block
+/***************************************************************/
+
+/***************************************************************/
+//The following block should also be moved to a library, but I still don't the time to figure this out atm.
+
+//CommsIn.h
+
+void commandDecoder(char* command) {
+ int8_t sign =1;
+ uint8_t index = 1;
+ int intValue = 0;
+ float decimalValue = 0;
+ switch (command[0]) {
+ case 'R' :
+ // Check sign
+ if (command[1] == '-'){
+ sign = -1;
+ index++;
+ }
+ // Take first digit
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = command[index] - '0';
+ index++;
+ }
+ else {
+ putMessage(INVALID_CMD);
+ break;
+ }
+
+ //Try taking 2 more digits
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = intValue*10 + command[index] - '0';
+ index++;
+ }
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = intValue*10 + command[index] - '0';
+ index++;
+ }
+ //Check for '.'
+ if (command[index] == '.') {
+ index++;
+ //Check for decimals
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ decimalValue = (command[index] - '0')/10;
+ index++;
+ }
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ decimalValue = (decimalValue/10 + command[index] - '0')/10;
+ }
+ }
+ putMessage(R_ECHO1, intValue);
+ putMessage(R_ECHO2, (int) (100*decimalValue));
+ decimalValue = (decimalValue + intValue) * sign;
+ //HERE SEND IT TO ANY GLOBAL VARIABLE YOU WISH
+ break;
+
+ case 'V' :
+ // Take first digit
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = command[index] - '0';
+ index++;
+ }
+ else {
+ putMessage(INVALID_CMD);
+ break;
+ }
+
+ //Try taking 2 more digits
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = intValue*10 + command[index] - '0';
+ index++;
+ }
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ intValue = intValue*10 + command[index] - '0';
+ index++;
+ }
+ //Check for '.'
+ if (command[index] == '.') {
+ index++;
+ //Check for one decimal
+ if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
+ decimalValue = (command[index] - '0')/10;
+ }
+ }
+ putMessage(MAX_SPEED_ECHO1, intValue);
+ putMessage(MAX_SPEED_ECHO2, (int) (100*decimalValue));
+ decimalValue = (decimalValue + intValue);
+ //HERE SEND IT TO ANY GLOBAL VARIABLE YOU WISH
+ // maxSpeed = decimalValue;
+ break;
+
+ case 'K' :
+ ///pc.printf("Received key: %016llx\n\r", mail->data);
+ break;
+ case 'T' :
+ //pc.printf("Received key: %016llx\n\r", mail->data);
+ break;
+ }
}
void commandProcessor() {
@@ -119,26 +306,25 @@
command[17] = '\0';
if (command[0] == 'R') {
- pc.printf("Rotation command\n");
-
- pc.printf("%s", command);
+ putMessage(ROTATION_CMD);
+ commandDecoder(command);
}
else if (command[0] == 'V') {
- pc.printf("Max speed command\n");
- pc.printf("%s", command);
+ putMessage(MAX_SPEED_CMD);
}
else if (command[0] == 'K') {
if (index == 18){ // when index is 18 means you entered K and 16 digits
number = command +1; //super bad solution, but I don't know how to work with strings in C
receivedKey = strtoull(number, NULL, 16);
+ putMessage(KEY_ECHO,receivedKey);
//receivedKey = 2147483648;
//sscanf(command, "%d", &receivedKey);
- pc.printf("Received key: %016llx\n\r", receivedKey);
+ //pc.printf("Received key: %016llx\n\r", receivedKey);
newKey_mutex.lock();
newKey = receivedKey;
newKey_mutex.unlock();
} else {
- pc.printf("Not a valid key!");
+ putMessage(KEY_DECLINED);
};
}
else if (command[0] == 'T') {
@@ -148,7 +334,7 @@
memset(command, 0, sizeof(command));
index = 0;
} else {
- pc.printf("Current command: %s\n\r", command);
+ pc.printf("Current command: %s\n\r", command); //Not sure how to go around this one cause it requires passing string
}
}
}
@@ -171,7 +357,7 @@
Timer t;
t.start();
unsigned currentTime = 0;
- unsigned currentCount = 0;
+ unsigned currentCount= 0;
for (unsigned i = 0; i <= UINT_MAX; i++) {
(*nonce)++;
@@ -185,7 +371,7 @@
}
if ((unsigned) t.read() == currentTime) {
//pc.printf("Hash rate: %d\n\r", i - currentCount);
- pc.printf("Current key: %016llx\n\r", *key);
+ //pc.printf("Current key: %016llx\n\r", *key);
currentTime++;
currentCount = i;
}
@@ -196,32 +382,32 @@
-void motorCtrlTick(){
- motorCtrlT.signal_set(0x1);
- }
+
//Set a given drive state
-void motorOut(int8_t driveState,uint32_t motorTorque){
+void motorOut(int8_t driveState){
//Lookup the output byte from the drive state.
int8_t driveOut = driveTable[driveState & 0x07];
//Turn off first
- if (~driveOut & 0x01) L1L.pulsewidth(0);
+ if (~driveOut & 0x01) L1L = 0;
if (~driveOut & 0x02) L1H = 1;
- if (~driveOut & 0x04) L2L.pulsewidth(0);
+ if (~driveOut & 0x04) L2L = 0;
if (~driveOut & 0x08) L2H = 1;
- if (~driveOut & 0x10) L3L.pulsewidth(0);
+ if (~driveOut & 0x10) L3L = 0;
if (~driveOut & 0x20) L3H = 1;
//Then turn on
- if (driveOut & 0x01) L1L.pulsewidth(motorTorque);
+ if (driveOut & 0x01) L1L = 1;
if (driveOut & 0x02) L1H = 0;
- if (driveOut & 0x04) L2L.pulsewidth(motorTorque);
+ if (driveOut & 0x04) L2L = 1;
if (driveOut & 0x08) L2H = 0;
- if (driveOut & 0x10) L3L.pulsewidth(motorTorque);
+ if (driveOut & 0x10) L3L = 1;
if (driveOut & 0x20) L3H = 0;
+
+
}
//Convert photointerrupter inputs to a rotor state
@@ -231,24 +417,23 @@
int8_t motorHome() {
//Put the motor in drive state 0 and wait for it to stabilize
- L1L.period(2000);
- L2L.period(2000);
- L3L.period(2000);
- motorOut(0,200);
+ motorOut(0);
wait(2.0);
return readRotorState();
}
-//orState is subtracted from future rotor state inputs to align rotor and motor states
-int8_t orState = motorHome();
+
// ISR to handle the updating of the motor position
void motorISR() {
static int8_t oldRotorState;
+ //orState is subtracted from future rotor state inputs to align rotor and motor states
+
int8_t rotorState = readRotorState();
- motorOut((rotorState-orState+lead+6)%6,pulseWidth); //+6 to make sure the remainder is positive
+ motorOut((rotorState-orState+lead+6)%6); //+6 to make sure the remainder is positive
if (rotorState - oldRotorState == 5) motorPosition --;
else if (rotorState - oldRotorState == -5) motorPosition ++;
else motorPosition += (rotorState - oldRotorState);
+ //pc.printf ("motorpPosition %f\n\r", motorPosition);
oldRotorState = rotorState;
}
/*void push() {
@@ -258,61 +443,89 @@
motorOut((intState - orState + lead +6) % 6); //+6 to make sure the remainder is positive
}
}*/
-
+void motorCtrlTick(){
+ motorCtrlT.signal_set(0x1);
+ }
void motorCtrlFn(){
int32_t counter=0;
static int32_t oldmotorPosition;
+ // uint32_t Ts;
+
+ // float Speed;
+ // float kps = 25; //proportional constant for speed
// Timer to count time passed between ticks to calculate velocity
Timer motorTime;
motorTime.start();
float motorPos;
- float windingSpeed;
- float windingRev;
-
+ //float ki = ??; // integration constant, to be tested for friction
Ticker motorCtrlTicker;
motorCtrlTicker.attach_us(&motorCtrlTick,100000);
while(1){
motorCtrlT.signal_wait(0x1);
+// errorSum= 0;
+// for(uint8_t i=9; i >0 ; i--){
+ // PrevErrorArray[i] = prevErrorArray[i-1];
+ // errorSum+= PrevErrorArray[i];
+ }
// convert state change into rotations
- windingSpeed = maxSpeed*6;
- windingRev = newRev*6;
+ //Speed = maxSpeed*6;
+
motorPos = motorPosition;
+ //pc.printf ("motor Pos: %f\n\r", motorPos);
motorVelocity = (motorPos - oldmotorPosition)/motorTime.read();
oldmotorPosition = motorPos;
+
+ //equation for controls
+ //Ts = kps*(Speed -abs(motorVelocity));//*errorSign;
+ //pc.printf ("torque: %f\n\r", Ts);
+ // Mp = ks*error + kd*(error - PrevError) /motorTime.read() + ki*errorSum;
+
motorTime.reset();
// Serial output to monitor speed and position
counter++;
if(counter == 10){
counter = 0;
//display velocity and motor position
- putMessage(3,(float)(motorPos/6.0));
- putMessage(4,(float)(motorVelocity/6.0));
+ // pc.printf ("motor Pos: %f\n\r", (motorPosition/6.0));
+ putMessage(MOTOR_POS,0,(float)(motorPos/6.0));
+ putMessage(MOTOR_SPEED,0,(float)(motorVelocity/6.0));
}
}
+
int main() {
//Serial pc(SERIAL_TX, SERIAL_RX);
//Initialise bincoin mining and communication
- bitcointhread.set_priority(osPriorityNormal);
- commandProcessorthread.set_priority(osPriorityHigh);
- commandProcessorthread.start(commandProcessor);
- bitcointhread.start(bitcoin);
+
+ d9.period(0.002f); //Set PWM period in seconds
+ d9.write(1);
+
+
//PWM.period(0.002f); //Set PWM period in seconds
//PWM.write(0.5); //Set PWM duty in %
+
- pc.printf("Hello Pete\n\r");
-
- orState = motorHome();
- pc.printf("Rotor origin: %x\n\r", orState);
+
+ commandProcessorthread.start(commandProcessor);
+
+ commsOut.start(commsOutFunc);
+
+ motorCtrlT.start(motorCtrlFn);
+ bitcointhread.start(bitcoin);
- I1.rise(&push);
- I2.rise(&push);
- I3.rise(&push);
+ putMessage(WELCOME);
+ int8_t orState = motorHome();
+ putMessage(ROTOR_ORG);
+ //pc.printf("Rotor origin: %x\n\r", orState);
+ //Set PWM duty in %
+ I1.rise(&motorISR);
+ I2.rise(&motorISR);
+ I3.rise(&motorISR);
- I1.fall(&push);
- I2.fall(&push);
- I3.fall(&push);
-
+ I1.fall(&motorISR);
+ I2.fall(&motorISR);
+ I3.fall(&motorISR);
+
}