![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is probably never gonna get done
Diff: main.cpp
- Revision:
- 20:5bd9f9e406d1
- Parent:
- 18:e48c0910c71e
- Child:
- 21:34f440ae0227
--- 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); + }