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:
- 10:808cb9052f14
- Parent:
- 9:061600a6c750
- Child:
- 11:74eeb8871fe6
diff -r 061600a6c750 -r 808cb9052f14 main.cpp
--- a/main.cpp Tue Nov 20 15:34:06 2018 +0000
+++ b/main.cpp Fri Dec 14 11:00:47 2018 +0000
@@ -1,7 +1,7 @@
#include "mbed.h"
#include "QEI.h"
-void Initialisation (void); //These voids are written after the main. They must be listed here too (functional prototypes).
+void Initialisation (void); //These voids are written after the main. They must be listed here too (functional prototypes).
void StepCW(void);
void Ph1(void);
void Ph12 (void);
@@ -15,50 +15,57 @@
void RPM (void);
void VelocityLoop (void);
-Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
+Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
-QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
+QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
-Timer t;
+Timer t; //timer used in RPM
-PwmOut Phase1 (p21); //Pin and LED set up
-PwmOut Phase2 (p22);
-PwmOut Phase3 (p23);
-PwmOut Phase4 (p24);
+PwmOut Phase1 (p23); //Pin and LED set up - originally standard pins but changed to PWM to enable speed control
+PwmOut Phase2 (p24);
+PwmOut Phase3 (p25);
+PwmOut Phase4 (p26);
-AnalogOut Aout(p18);
+AnalogOut Aout(p18); //Used with multimeter to give a speed indicator 1mV = 1RPM
-DigitalIn Button1 (p11);
-DigitalIn Button2 (p12);
+//DigitalIn Button1 (p11); //not used
+//DigitalIn Button2 (p12); //not used
-DigitalOut led1(LED1);
+DigitalOut led1(LED1); //LEDs used to as very basic memmory for controlling the state machines
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
-int StateA = 0;
-int StateB = 0;
+DigitalOut SerialClock (p12); //ReadKType
+DigitalIn DOut (p13); //ReadKType
+DigitalOut cs1 (p14); //ReadKType
+
+int StateA = 0; //State for first 2 revolutions (calibration of the index)
+int StateB = 0; //All state machines after calibration use this state
//int StateC = 0;
-int AdjCW = 3;
-int AdjACW = 6;
-int TimePerClick = 0;
-int TimePerRev = 0;
-int RPS = 0;
-int rpm = 0;
-int SetPoint = 500;
-int z = 80;
+int AdjCW = 0; //CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
+int AdjACW = 3; //ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
+int TimePerClick = 0; //for calc of RPM
+int TimePerRev = 0; //for calc of RPM
+int RPS = 0; //for calc of RPMl;
+int rpm = 0; //for calc of RPM
+int SetPoint = 1000; //for adjusting the speed
+int z = 80; //TimePerRev = TimePerClick * (800/z); 800 pulses per rev, PulseCount2_==80 for wheel.getwhoop_ flag. i.e. 10 points per reoluition for RPM calc.
+int i = 0; //ReadKType
+int Readout = 0; //ReadKType
-char c;
+char c; //keyboard cotrol GetChar
-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
+float duty = 0.5; //velocity loop: 1 = fastest, 0.96 = slowest. Below 0.96 the motor will not operate.
+float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
+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
+float temp = 0; //ReadKType
int main(void)
{
- pc.baud(230400); //Set fastest baud rate
- Phase1.period(0.00002);
+ pc.baud(230400); //Set fastest baud rate
+ Phase1.period(0.00002); //period of 0.000002 = 2 microseconds (50kHz). Good balance of low and high speed performance.
Phase2.period(0.00002);
Phase3.period(0.00002);
Phase4.period(0.00002);
@@ -66,9 +73,8 @@
Initialisation();
wait(0.1);
t.start();
-
-
- while(wheel.getRevolutions()<2)
+
+while(wheel.getRevolutions()<2) //Index Calibration
{
switch(StateA)
{
@@ -91,7 +97,7 @@
default:break;
}
- if(wheel.getYay()==1)
+ if(wheel.getYay()==1) //PulseCount_==1, yay_=1;
{
StateA++;
wheel.ResetYay();
@@ -104,20 +110,21 @@
while(1)
{
- while((led1 == 0) && (led2 == 0))
+ while((led1 == 0) && (led2 == 0)) //If no command to operate
{
Aout = 0;
+ rpm = 0;
Phase1.write(0);
Phase2.write(0);
Phase3.write(0);
Phase4.write(0);
- GetChar();
+ 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()>1) && (wheel.getPulses()>0) && (led1==1))
+ while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1)) //After Calibration, Prev CW movement, CW command
{
GetChar();
StateB = (wheel.getPulses()+StateA+AdjCW)%16;
@@ -146,14 +153,14 @@
default:break;
}
- if(wheel.getWhoop()==1)
+ if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
{
RPM();
VelocityLoop();
}
}
- while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1))
+ while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1)) //After Calibration, Prev ACW movement, CW command
{
GetChar();
StateB = (800+wheel.getPulses()+StateA+AdjCW)%16;
@@ -181,13 +188,13 @@
default:break;
}
- if(wheel.getWhoop()==1)
+ if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
{
RPM();
VelocityLoop();
}
}
- while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1))
+ while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1)) //After Calibration, Prev CW movement, ACW command
{
GetChar();
//StateB = (800+wheel.getPulses())%16;
@@ -215,13 +222,13 @@
default:break;
}
- if(wheel.getWhoop()==1)
+ if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
{
RPM();
VelocityLoop();
}
}
- while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1))
+ while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1)) //After Calibration, Prev ACW movement, ACW command
{
GetChar();
StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
@@ -247,7 +254,7 @@
case 0:Ph1();break;
default:break;
}
- if(wheel.getWhoop()==1)
+ if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
{
RPM();
VelocityLoop();
@@ -350,7 +357,7 @@
//wait(y);
}
-void Initialisation (void)
+void Initialisation (void) //Turn everything off
{
Phase1.write(0);
Phase2.write(0);
@@ -363,62 +370,126 @@
wheel.ResetYay();
}
-void GetChar (void)
+void GetChar (void) //read keyboard strikes with terraterm
{ if (pc.readable())
{
c = pc.getc();
- if(c == 'z')
+ if(c == 'z') //turn on led1 causes CW operation
{
led1 = !led1;
led2 = 0;
+ pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
- if(c == 'x')
+ if(c == 'x') //turn on led2 causes ACW operation
{
led1 = 0;
led2 = !led2 ;
+ pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
- if(c == 'q')
+ if(c == 'q') //Increases setpoint used in Velocity loop
{
//duty = duty + 0.0001;
- SetPoint=SetPoint+10;
+ SetPoint=SetPoint+10;
+ if (SetPoint >2200)
+ {
+ SetPoint = 2200;
+ }
+ pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
- if(c == 'a')
+ if(c == 'a') //Decreases setpoint used in Velocity loop
{
//duty = duty - 0.0001;
- SetPoint=SetPoint-10;
- if (SetPoint <500)
+ SetPoint=SetPoint-10;
+ if (SetPoint <50)
{
- SetPoint = 500;
+ SetPoint = 50;
}
+ pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
+ }
+ if(c== 'o')
+ {
+ AdjCW = AdjCW+1;
+ if (AdjCW >15)
+ {
+ AdjCW = 0;
+ }
+ pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
+ }
+ if(c== 'k')
+ {
+ AdjCW = AdjCW-1;
+ if (AdjCW <0)
+ {
+ AdjCW = 15;
+ }
+ pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
+ }
+ if(c== 'p')
+ {
+ AdjACW = AdjACW+1;
+ if (AdjACW >15)
+ {
+ AdjACW = 0;
+ }
+ pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
+ }
+ if(c== 'l')
+ {
+ AdjACW = AdjACW-1;
+ if (AdjACW <0)
+ {
+ AdjACW = 15;
+ }
+ pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
+ }
+ if(c=='0')
+ {
+ pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
}
}
void RPM (void)
{
- wheel.ResetWhoop();
- TimePerClick = (t.read_us());
- t.reset();
- TimePerRev = TimePerClick * (800/z);
- TimePerRev = TimePerRev / 1000;
+ wheel.ResetWhoop(); //PulseCount2_==80, whoop_=1;
+ TimePerClick = (t.read_us()); //read timer in microseconds
+ t.reset(); //reset timer
+ TimePerRev = TimePerClick * (800/z); //z = 80 (PulseCount2_==80)
+ TimePerRev = TimePerRev / 1000; //
RPS = 10000000 / TimePerRev;
rpm = (RPS * 60)/10000;
- Aout=((0.298*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
- //pc.printf("rpm = %i\n\r", rpm);
+ Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
+ //if(rpm < 800)
+ //{
+ // AdjCW = 0;
+ // AdjACW = 3;
+ //}
+ //if(rpm > 799 and rpm < 2000)
+ //{
+ // AdjCW = 1;
+ // AdjACW = 2;
+ //}
+ //if(rpm >1999)
+ //{
+ // AdjCW = 2;
+ // AdjACW = 1;
+ //}
+
+ //pc.printf("rpm = %i\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.00001);
- if (duty > 1)
+ diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
+ duty = duty + (diff*0.00001); //duty is adjusted to speed up or slow down until difference = 0
+ if (duty > 1) //limits for duty. Motor will not operate below 0.96. 1 = max
{
duty = 1;
}
- if (duty <0.96)
- {
- duty = 0.96;
+ if (duty <0.01) //3A min duty 0.96, 6.5A min duty 0.4
+ {
+ duty = 0.01;
}
- pc.printf("%i, %.5f\r", SetPoint, duty); //SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
+ //pc.printf("%i, %.5f, %i, %i, %i \r", SetPoint, duty, AdjCW, AdjACW, rpm); //SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
}
\ No newline at end of file