Rotork Research Team
/
TFM_Encoder
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 7:b8de1529c7fc
- Parent:
- 6:f7028034aabb
- Child:
- 8:2e690f407ec6
--- a/main.cpp Thu Nov 15 15:57:10 2018 +0000
+++ b/main.cpp Mon Nov 19 11:14:28 2018 +0000
@@ -13,6 +13,7 @@
void Ph41 (void);
void GetChar (void);
void RPM (void);
+void VelocityLoop (void);
Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
@@ -20,10 +21,10 @@
Timer t;
-DigitalOut Phase1 (p21); //Pin and LED set up
-DigitalOut Phase2 (p22);
-DigitalOut Phase3 (p23);
-DigitalOut Phase4 (p24);
+PwmOut Phase1 (p21); //Pin and LED set up
+PwmOut Phase2 (p22);
+PwmOut Phase3 (p23);
+PwmOut Phase4 (p24);
AnalogOut Aout(p18);
@@ -44,21 +45,30 @@
int TimePerRev = 0;
int RPS = 0;
int rpm = 0;
+int SetPoint = 250;
+int z = 80;
char c;
-float x=0.05; //x=time of square wave when 1 phase energised,
-float y=0.02; //y=time of square wave when 2 phases energised
+float duty = 1;
+float diff = 0.0;
+float x=0.1; //x=time of square wave when 1 phase energised,
+float y=0.04; //y=time of square wave when 2 phases energised
int main(void)
{
pc.baud(230400); //Set fastest baud rate
+ Phase1.period(0.000001);
+ Phase2.period(0.000001);
+ Phase3.period(0.000001);
+ Phase4.period(0.000001);
StepCW();
Initialisation();
wait(0.1);
t.start();
- while(wheel.getRevolutions()==0)
+
+ while(wheel.getRevolutions()<2)
{
switch(StateA)
{
@@ -96,20 +106,22 @@
{
while((led1 == 0) && (led2 == 0))
{
- Phase1 = 0;
- Phase2 = 0;
- Phase3 = 0;
- Phase4 = 0;
- GetChar();
+ Aout = 0;
+ Phase1.write(0);
+ Phase2.write(0);
+ Phase3.write(0);
+ Phase4.write(0);
+ GetChar();
//StateB = wheel.getPulses()%16;
//StateC = (800+wheel.getPulses()+StateA+AdjCW)%16;
//pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
//pc.printf("0 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
}
- while((wheel.getRevolutions()>0) && (wheel.getPulses()>0) && (led1==1))
+ while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1))
{
GetChar();
StateB = (wheel.getPulses()+StateA+AdjCW)%16;
+ //pc.printf("rpm = %i, Whoop = %i\n\r", rpm, wheel.getWhoop());
//pc.printf("StateB= %i\n\r", StateB);
//pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
@@ -134,13 +146,14 @@
default:break;
}
- if(wheel.getYay()==1)
+ if(wheel.getWhoop()==1)
{
RPM();
+ //VelocityLoop();
}
}
- while(wheel.getRevolutions()>0 && wheel.getPulses()<1 && (led1==1))
+ while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1))
{
GetChar();
StateB = (800+wheel.getPulses()+StateA+AdjCW)%16;
@@ -168,12 +181,13 @@
default:break;
}
- if(wheel.getYay()==1)
+ if(wheel.getWhoop()==1)
{
RPM();
+ //VelocityLoop();
}
}
- while((wheel.getRevolutions()>0) && (wheel.getPulses()>0) && (led2==1))
+ while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1))
{
GetChar();
//StateB = (800+wheel.getPulses())%16;
@@ -201,12 +215,13 @@
default:break;
}
- if(wheel.getYay()==1)
+ if(wheel.getWhoop()==1)
{
RPM();
+ //VelocityLoop();
}
}
- while((wheel.getRevolutions()>0) && (wheel.getPulses()<1) && (led2==1))
+ while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1))
{
GetChar();
StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
@@ -232,9 +247,10 @@
case 0:Ph1();break;
default:break;
}
- if(wheel.getYay()==1)
+ if(wheel.getWhoop()==1)
{
RPM();
+ //VelocityLoop();
}
}
}
@@ -261,69 +277,85 @@
void Ph1(void)
{
- Phase1 = 1;
- Phase2 = Phase3 = Phase4 = 0;
+ Phase1.write(duty);
+ Phase2.write(0);
+ Phase3.write(0);
+ Phase4.write(0);
//wait(x);
//pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
}
void Ph12 (void)
{
- Phase1 = Phase2 = 1;
- Phase3 = Phase4 = 0;
+ Phase1.write(duty);
+ Phase2.write(duty);
+ Phase3.write(0);
+ Phase4.write(0);
//wait(y);
}
void Ph2(void)
{
- Phase2 = 1;
- Phase1 = Phase3 = Phase4 = 0;
+ Phase1.write(0);
+ Phase2.write(duty);
+ Phase3.write(0);
+ Phase4.write(0);
//wait(x);
//pc.printf("Phase 2 = %i\n\r", wheel.getPulses());
}
void Ph23 (void)
{
- Phase2 = Phase3 = 1;
- Phase4 = Phase1 = 0;
+ Phase1.write(0);
+ Phase2.write(duty);
+ Phase3.write(duty);
+ Phase4.write(0);
//wait(y);
}
void Ph3(void)
{
- Phase3 = 1;
- Phase1 = Phase2 = Phase4 = 0;
+ Phase1.write(0);
+ Phase2.write(0);
+ Phase3.write(duty);
+ Phase4.write(0);
//wait(x);
//pc.printf("Phase 3 = %i\n\r", wheel.getPulses());
}
void Ph34 (void)
{
- Phase3 = Phase4 = 1;
- Phase1 = Phase2 = 0;
+ Phase1.write(0);
+ Phase2.write(0);
+ Phase3.write(duty);
+ Phase4.write(duty);
//wait(y);
}
void Ph4(void)
{
- Phase4 = 1;
- Phase1 = Phase2 = Phase3 = 0;
+ Phase1.write(0);
+ Phase2.write(0);
+ Phase3.write(0);
+ Phase4.write(duty);
//wait(x);
//pc.printf("Phase 4 = %i\n\r", wheel.getPulses());
}
void Ph41 (void)
{
- Phase4 = Phase1 = 1;
- Phase2 = Phase3 = 0;
+ Phase1.write(duty);
+ Phase2.write(0);
+ Phase3.write(0);
+ Phase4.write(duty);
//wait(y);
}
void Initialisation (void)
{
- Phase1 = 0;
- Phase2 = 0;
- Phase3 = 0;
- Phase4 = 0;
+ Phase1.write(0);
+ Phase2.write(0);
+ Phase3.write(0);
+ Phase4.write(0);
led1 = 0;
led2 = 0;
led3 = 0;
@@ -345,19 +377,46 @@
led1 = 0;
led2 = !led2 ;
}
+ if(c == 'q')
+ {
+ SetPoint=SetPoint+10;
+ }
+ if(c == 'a')
+ {
+ SetPoint=SetPoint-10;
+ //if (Setpoint <100)
+ //{
+ // SetPoint = 100;
+ // }
+ }
}
}
void RPM (void)
{
- wheel.ResetYay();
+ wheel.ResetWhoop();
TimePerClick = (t.read_us());
t.reset();
- TimePerRev = TimePerClick * 800;
+ TimePerRev = TimePerClick * (800/z);
TimePerRev = TimePerRev / 1000;
RPS = 10000000 / TimePerRev;
rpm = (RPS * 60)/10000;
- Aout=((0.30303*rpm)/1000); // for 500 rpm (0.3030*500/1000)*3.3V = 0.500V
- //pc.printf("rpm = %d\n\r", rpm);
+ Aout=((0.298*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
+ //pc.printf("rpm = %i\n\r", rpm);
//pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
+}
+
+void VelocityLoop (void)
+{
+ diff = SetPoint - rpm;
+ duty = duty + (diff*0.00005);
+ if (duty > 0.9)
+ {
+ duty = 0.9;
+ }
+ if (duty <0.1)
+ {
+ duty = 0.1;
+ }
+ pc.printf("duty = %f, SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
}
\ No newline at end of file