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:
- 17:19b2c598810a
- Parent:
- 16:e6c8df9960c6
- Child:
- 18:3493de6fe8ce
--- a/main.cpp Wed Feb 06 15:46:51 2019 +0000
+++ b/main.cpp Wed Apr 03 10:46:47 2019 +0000
@@ -23,11 +23,11 @@
Timer t; //timer used in RPM
-PwmOut Phase1 (p21); //Pin and LED set up - originally standard pins but changed to PWM to enable speed control
-PwmOut Phase2 (p22);
-PwmOut Phase3 (p23);
-PwmOut Phase4 (p24);
-DigitalOut UnUsedPhase1 (p25);
+DigitalOut UnUsedPhase1 (p21);
+PwmOut Phase1 (p22); //Pin and LED set up - originally standard pins but changed to PWM to enable speed control
+PwmOut Phase2 (p23);
+PwmOut Phase3 (p24);
+PwmOut Phase4 (p25);
DigitalOut UnUsedPhase2 (p26);
//AnalogOut Aout(p18); //Used with multimeter to give a speed indicator 1mV = 1RPM
@@ -53,8 +53,8 @@
int TimePerClick = 0; //for calc of RPM
int RPS = 0; //for calc of RPM
int rpm = 0; //for calc of RPM
-int SetPoint = 1500; //for adjusting the speed
-int enc = 3200;
+int SetPoint = 1000; //for adjusting the speed
+int enc = 3200; //800 x4 enc = 3200 Pulses Per Rev
int i = 0;
int j = 0;
int k = 0;
@@ -62,15 +62,15 @@
int m = 0;
int n = 0;
int s = enc/50;
-int z = 3200; //TimePerRev = TimePerClick * (800/z); 800 pulses per rev, PulseCount2_==800 for wheel.getwhoop_ flag. i.e. 10 points per reoluition for RPM calc.
+int z = 3200; //TimePerRev = TimePerClick * (3200/z); 3200 pulses per rev, PulseCount2_==3200 for wheel.getwhoop_ flag. i.e. 1 points per reoluition for RPM calc.
int T = 80; //Motor temp limit
int slowloop = 0;
char c; //keyboard cotrol GetChar
-float duty = 1; //velocity loop: 1 = fastest, 0.96 = slowest. Below 0.96 the motor will not operate.
+float duty = 1;
float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
-float AdjDiff = 0.00001;
+float AdjDiff = 0.0001;
float p = 0.000014;
float x=0.1; //x=time of square wave when 1 phase energised,
float TimePerRev = 0; //for calc of RPM
@@ -79,7 +79,7 @@
int main(void)
{
- pc.baud(230400); //Set fastest baud rate
+ pc.baud(921600); //Set fastest baud rate
Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
Phase2.period(p);
Phase3.period(p);
@@ -91,8 +91,7 @@
Initialisation();
/*while(1)
-{
-
+{
}
*/
@@ -741,6 +740,30 @@
{
pc.printf(" 0 = Phase Mapping\n\rq = setpoint+, w = duty+, e = AdjDiff+, t = temp, y = states, o = AdjCW+, p = AdjACW+,\n\r a = setpoint-, s = duty-, d = AdjDiff-, k = AdjCW-, l = AdjACW-,\n\r z = CW, x = ACW\n\r");
}
+ if(c == '1')
+ {
+ Ph1();
+ }
+ if(c == '2')
+ {
+ Ph2();
+ }
+ if(c == '3')
+ {
+ Ph3();
+ }
+ if(c == '4')
+ {
+ Ph4();
+ }
+ if(c == '5')
+ {
+ Ph0();
+ }
+ if(c == '6')
+ {
+ StepACW();
+ }
if(c == 'z') //turn on led1 causes CW operation
{
led1 = !led1;
@@ -765,9 +788,9 @@
if(c == 'a') //Decreases setpoint used in Velocity loop
{
SetPoint=SetPoint-5;
- if (SetPoint <50)
+ if (SetPoint <0)
{
- SetPoint = 50;
+ SetPoint = 0;
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
@@ -872,10 +895,10 @@
void RPM (void)
{
- wheel.ResetWhoop(); //PulseCount2_==400 x 4, x4 encoding, whoop_=1;
+ wheel.ResetWhoop(); //Whoop = 1 x per rev
TimePerClick = (t.read_us()); //read timer in microseconds
t.reset(); //reset timer
- TimePerRev = enc * TimePerClick / z; //z = 400 (PulseCount2_==1600)
+ TimePerRev = TimePerClick; //enc * TimePerClick / z; //enc = 3200, z = 3200 (PulseCount2_==1600)
TimePerRev = TimePerRev / 1000000; // 1microsecond = 0.000001s
RPS = 1 / TimePerRev; //inverse to convert SPR to RPS
rpm = RPS * 60; //x 60 to convert RPS to RPM
@@ -886,7 +909,7 @@
if(rpm < 65)
{
AdjCW = (57);
- AdjACW = (12);
+ AdjACW = (14);
}
if(rpm > 64 and rpm <75)
{
@@ -941,7 +964,7 @@
if(rpm > 974 and rpm < 1150)
{
AdjCW = (4);
- AdjACW = (1-n);
+ AdjACW = (1);
}
if(rpm > 1149 and rpm < 1375)
{
@@ -1004,543 +1027,14 @@
if(rpm < 175)
{
AdjCW = (60);
- AdjACW = (11);
+ AdjACW = (14);
}
- if(rpm > 200 and rpm < 275)
- {
- AdjCW = (5);
- AdjACW = (11);
- }if(rpm > 300 and rpm < 1000)
+ if(rpm > 200 and rpm < 1000)
{
AdjCW = (5);
AdjACW = (5);
}
}
- /*if(rpm < 180)
- {
- AdjCW = (57+n);
- AdjACW = (12-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 179 and rpm <195)
- {
- AdjCW = (58+n);
- AdjACW = (11-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 194 and rpm <220)
- {
- AdjCW = (59+n);
- AdjACW = (10-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 219 and rpm <240)
- {
- AdjCW = (60+n);
- AdjACW = (9-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 239 and rpm <295)
- {
- AdjCW = (61+n);
- AdjACW = (8-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 294 and rpm <325)
- {
- AdjCW = (62+n);
- AdjACW = (7-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 324 and rpm <355)
- {
- AdjCW = (63+n);
- AdjACW = (6-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 354 and rpm <415)
- {
- AdjCW = (0+n);
- AdjACW = (5-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 414 and rpm <595)
- {
- AdjCW = (1+n);
- AdjACW = (4-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 594 and rpm <655)
- {
- AdjCW = (2+n);
- AdjACW = (3-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 654 and rpm <765)
- {
- AdjCW = (3+n);
- AdjACW = (2-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 764 and rpm <1010)
- {
- AdjCW = (4+n);
- AdjACW = (1-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 1009 and rpm <1150)
- {
- AdjCW = (5+n);
- AdjACW = (0-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 1149 and rpm <1225)
- {
- AdjCW = (6+n);
- AdjACW = (63-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 1224 and rpm <1310)
- {
- AdjCW = (7+n);
- AdjACW = (62-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 1309 and rpm <1670)
- {
- AdjCW = (8+n);
- AdjACW = (61-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 1669 and rpm <2035)
- {
- AdjCW = (9+n);
- AdjACW = (60-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 2034 and rpm <2175)
- {
- AdjCW = (10+n);
- AdjACW = (59-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 2174 and rpm <2275)
- {
- AdjCW = (11+n);
- AdjACW = (58-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 2274 and rpm <2515)
- {
- AdjCW = (12+n);
- AdjACW = (57-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 2514 and rpm <2575)
- {
- AdjCW = (13+n);
- AdjACW = (56-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 2574 and rpm <2755)
- {
- AdjCW = (14+n);
- AdjACW = (55-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 2754 and rpm <2835)
- {
- AdjCW = (15+n);
- AdjACW = (54-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 2834 and rpm <2860)
- {
- AdjCW = (16+n);
- AdjACW = (53-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }
- if(rpm > 2861)
- {
- AdjCW = (17+n);
- AdjACW = (52-n);
- if (AdjCW >(s-1))
- {
- AdjCW = 0;
- }
- if (AdjCW <0)
- {
- AdjCW = (s-1);
- }
- if (AdjACW <0)
- {
- AdjACW = (s-1);
- }
- if (AdjACW >(s-1))
- {
- AdjACW = 0;
- }
- }*/
//pc.printf("RPS = %i \r", RPS);
//pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
}