![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
An embedded device
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 18:e48c0910c71e
- Parent:
- 17:ff5300ba5442
- Child:
- 20:5bd9f9e406d1
--- a/main.cpp Sat Mar 16 15:53:32 2019 +0000 +++ b/main.cpp Mon Mar 18 16:30:12 2019 +0000 @@ -55,21 +55,25 @@ InterruptIn I3(I3pin); //Motor Drive outputs -DigitalOut L1L(L1Lpin); +PwmOut L1L(L1Lpin); DigitalOut L1H(L1Hpin); -DigitalOut L2L(L2Lpin); +PwmOut L2L(L2Lpin); DigitalOut L2H(L2Hpin); -DigitalOut L3L(L3Lpin); +PwmOut L3L(L3Lpin); DigitalOut L3H(L3Hpin); -PwmOut PWM(PWMpin); + int8_t orState = 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 typedef struct { uint64_t nonce; + float data; } mail_t; Mail<mail_t, 16> mail_box; @@ -80,9 +84,17 @@ Mutex newKey_mutex; uint64_t newKey = 0; -void putMessage(uint64_t *nonce){ +volatile float newRev; +volatile float maxSpeed = 300; +uint32_t pulseWidth; +float motorPosition_command; +float motorPosition; + +// mail to queue messages for serial port +void putMessage(uint64_t *nonce,float data){ mail_t *mail = mail_box.alloc(); mail->nonce = *nonce; + mail->data = *data; mail_box.put(mail); } @@ -108,6 +120,7 @@ if (command[0] == 'R') { pc.printf("Rotation command\n"); + pc.printf("%s", command); } else if (command[0] == 'V') { @@ -180,26 +193,34 @@ t.stop(); } } + + + +void motorCtrlTick(){ + motorCtrlT.signal_set(0x1); + } + + //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 & 0x01) L1L.pulsewidth(0); if (~driveOut & 0x02) L1H = 1; - if (~driveOut & 0x04) L2L = 0; + if (~driveOut & 0x04) L2L.pulsewidth(0); if (~driveOut & 0x08) L2H = 1; - if (~driveOut & 0x10) L3L = 0; + if (~driveOut & 0x10) L3L.pulsewidth(0); if (~driveOut & 0x20) L3H = 1; //Then turn on - if (driveOut & 0x01) L1L = 1; + if (driveOut & 0x01) L1L.pulsewidth(motorTorque); if (driveOut & 0x02) L1H = 0; - if (driveOut & 0x04) L2L = 1; + if (driveOut & 0x04) L2L.pulsewidth(motorTorque); if (driveOut & 0x08) L2H = 0; - if (driveOut & 0x10) L3L = 1; + if (driveOut & 0x10) L3L.pulsewidth(motorTorque); if (driveOut & 0x20) L3H = 0; } @@ -209,19 +230,65 @@ } int8_t motorHome() { - motorOut(0); + //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); wait(2.0); return readRotorState(); } -void push() { +//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; + int8_t rotorState = readRotorState(); + 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 push() { intState = readRotorState(); if (intState != intStateOld) { intStateOld = intState; motorOut((intState - orState + lead +6) % 6); //+6 to make sure the remainder is positive } -} +}*/ +void motorCtrlFn(){ + int32_t counter=0; + static int32_t oldmotorPosition; + // Timer to count time passed between ticks to calculate velocity + Timer motorTime; + motorTime.start(); + float motorPos; + float windingSpeed; + float windingRev; + + Ticker motorCtrlTicker; + motorCtrlTicker.attach_us(&motorCtrlTick,100000); + while(1){ + motorCtrlT.signal_wait(0x1); + // convert state change into rotations + windingSpeed = maxSpeed*6; + windingRev = newRev*6; + motorPos = motorPosition; + motorVelocity = (motorPos - oldmotorPosition)/motorTime.read(); + oldmotorPosition = motorPos; + 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)); + } + } int main() { //Serial pc(SERIAL_TX, SERIAL_RX); @@ -231,8 +298,8 @@ commandProcessorthread.start(commandProcessor); bitcointhread.start(bitcoin); - PWM.period(0.002f); //Set PWM period in seconds - PWM.write(0.5); //Set PWM duty in % + //PWM.period(0.002f); //Set PWM period in seconds + //PWM.write(0.5); //Set PWM duty in % pc.printf("Hello Pete\n\r");