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:
- 14:1eb49362a607
- Parent:
- 13:da9d3fbbe407
- Child:
- 15:12a4bbfa6de4
--- a/main.cpp Thu Jan 10 15:07:19 2019 +0000
+++ b/main.cpp Fri Jan 25 15:04:56 2019 +0000
@@ -25,8 +25,10 @@
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);
+PwmOut Phase3 (p25);
+PwmOut Phase4 (p26);
+DigitalOut UnUsedPhase1 (p23);
+DigitalOut UnUsedPhase2 (p24);
//AnalogOut Aout(p18); //Used with multimeter to give a speed indicator 1mV = 1RPM
@@ -37,8 +39,6 @@
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
-DigitalOut UnUsedPhase1 (p25);
-DigitalOut UnUsedPhase2 (p26);
DigitalOut SerialClock (p12); //ReadKType
DigitalIn DOut (p13); //ReadKType
DigitalOut cs1 (p14); //ReadKType
@@ -46,95 +46,159 @@
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 = 2; //CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
-int AdjACW = 5; //ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
+int AdjCW = 57; //2 CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
+int AdjACW = 12; //5 ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
+int CW = 57;
+int ACW = 12;
int TimePerClick = 0; //for calc of RPM
-int TimePerRev = 0; //for calc of RPM
int RPS = 0; //for calc of RPM
int rpm = 0; //for calc of RPM
-int SetPoint = 700; //for adjusting the speed
-int z = 800; //TimePerRev = TimePerClick * (800/z); 800 pulses per rev, PulseCount2_==800 for wheel.getwhoop_ flag. i.e. 10 points per reoluition for RPM calc.
+int SetPoint = 1500; //for adjusting the speed
+int enc = 3200;
+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 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 diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
+float AdjDiff = 0.0001;
float x=0.1; //x=time of square wave when 1 phase energised,
+float TimePerRev = 0; //for calc of RPM
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); //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);
+ Phase1.period(0.00001); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
+ Phase2.period(0.00001);
+ Phase3.period(0.00001);
+ Phase4.period(0.00001);
+ wait(0.1);
+ t.start();
+ SerialClock = 0;
StepCW();
Initialisation();
- wait(0.1);
- t.start();
- SerialClock = 0;
-
+
+/*while(1)
+{
+
+}
+*/
+
while(wheel.getRevolutions()<2) //Index Calibration
- {
+ {
+ StateA = (wheel.getPulses()+25)%s;
switch(StateA)
{
- case 0:Ph1();break;
+ case 0:Ph1();break;//;pc.printf("1 Pulses= %i\n\r", wheel.getPulses());break;
case 1:Ph1();break;
- case 2:Ph12();break;
- case 3:Ph12();break;
- case 4:Ph2();break;
- case 5:Ph2();break;
- case 6:Ph23();break;
- case 7:Ph23();break;
- case 8:Ph3();break;
- case 9:Ph3();break;
- case 10:Ph34();break;
- case 11:Ph34();break;
- case 12:Ph4();break;
- case 13:Ph4();break;
- case 14:Ph41();break;
- case 15:Ph41();break;
+ case 2:Ph1();break;
+ case 3:Ph1();break;
+ case 4:Ph1();break;
+ case 5:Ph1();break;
+ case 6:Ph1();break;
+ case 7:Ph1();break;
+ case 8:Ph1();break;
+ case 9:Ph1();break;
+ case 10:Ph1();break;
+ case 11:Ph1();break;
+ case 12:Ph1();break;
+ case 13:Ph1();break;
+ case 14:Ph1();break;
+ case 15:Ph1();break;
+ case 16:Ph2();break;//;pc.printf("2 Pulses= %i\n\r", wheel.getPulses());break;
+ case 17:Ph2();break;
+ case 18:Ph2();break;
+ case 19:Ph2();break;
+ case 20:Ph2();break;
+ case 21:Ph2();break;
+ case 22:Ph2();break;
+ case 23:Ph2();break;
+ case 24:Ph2();break;
+ case 25:Ph2();break;
+ case 26:Ph2();break;
+ case 27:Ph2();break;
+ case 28:Ph2();break;
+ case 29:Ph2();break;
+ case 30:Ph2();break;
+ case 31:Ph2();break;
+ case 32:Ph3();break;//;pc.printf("3 Pulses= %i\n\r", wheel.getPulses());break;
+ case 33:Ph3();break;
+ case 34:Ph3();break;
+ case 35:Ph3();break;
+ case 36:Ph3();break;
+ case 37:Ph3();break;
+ case 38:Ph3();break;
+ case 39:Ph3();break;
+ case 40:Ph3();break;
+ case 41:Ph3();break;
+ case 42:Ph3();break;
+ case 43:Ph3();break;
+ case 44:Ph3();break;
+ case 45:Ph3();break;
+ case 46:Ph3();break;
+ case 47:Ph3();break;
+ case 48:Ph4();break;//;pc.printf("4 Pulses= %i\n\r", wheel.getPulses());break;
+ case 49:Ph4();break;
+ case 50:Ph4();break;
+ case 51:Ph4();break;
+ case 52:Ph4();break;
+ case 53:Ph4();break;
+ case 54:Ph4();break;
+ case 55:Ph4();break;
+ case 56:Ph4();break;
+ case 57:Ph4();break;
+ case 58:Ph4();break;
+ case 59:Ph4();break;
+ case 60:Ph4();break;
+ case 61:Ph4();break;
+ case 62:Ph4();break;
+ case 63:Ph4();break;
default:break;
}
-
if(wheel.getYay()==1) //PulseCount_==1, yay_=1;
{
StateA++;
wheel.ResetYay();
- if (StateA>15)
+ if (StateA>(s-1))
{
StateA=0;
}
}
- }
+ }
while(1)
{
while((led1 == 0) && (led2 == 0)) //If no command to operate
{
//Aout = 0;
+ //duty = 0.5;
rpm = 0;
- Phase1.write(0);
- Phase2.write(0);
- Phase3.write(0);
- Phase4.write(0);
+ AdjCW = CW;
+ AdjACW = ACW;
+ Ph0();
GetChar();
ReadKType();
- //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());
+ StateB = (wheel.getPulses()+StateA)%s; //wheel.getPulses()%1(s-1);
+ //StateC = (800+wheel.getPulses()+StateA+AdjCW)%s;
+ //if(wheel.getPulses()==wheel.getPulses()+1);
+ //{
+ //pc.printf("B StateA= %i, Pulses= %i, Revs= %i\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ //pc.printf("StateA= %i, StateB= %i, Pulses = %i \r", StateA, StateB, 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) && (temp<T)) //After Calibration, Prev CW movement, CW command
{
GetChar();
- StateB = (wheel.getPulses()+StateA+AdjCW)%16;
- //pc.printf("rpm = %i, Whoop = %i\n\r", rpm, wheel.getWhoop());
+ StateB = (wheel.getPulses()+StateA+AdjCW)%s;
+ //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());
+ //pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
switch(StateB)
{
@@ -142,34 +206,91 @@
case 1:Ph1();break;
case 2:Ph1();break;
case 3:Ph1();break;
- case 4:Ph2();break;
- case 5:Ph2();break;
- case 6:Ph2();break;
- case 7:Ph2();break;
- case 8:Ph3();break;
- case 9:Ph3();break;
- case 10:Ph3();break;
- case 11:Ph3();break;
- case 12:Ph4();break;
- case 13:Ph4();break;
- case 14:Ph4();break;
- case 15:Ph4();break;
+ case 4:Ph1();break;
+ case 5:Ph1();break;
+ case 6:Ph1();break;
+ case 7:Ph1();break;
+ case 8:Ph1();break;
+ case 9:Ph1();break;
+ case 10:Ph1();break;
+ case 11:Ph1();break;
+ case 12:Ph1();break;
+ case 13:Ph1();break;
+ case 14:Ph1();break;
+ case 15:Ph1();break;
+ case 16:Ph2();break;
+ case 17:Ph2();break;
+ case 18:Ph2();break;
+ case 19:Ph2();break;
+ case 20:Ph2();break;
+ case 21:Ph2();break;
+ case 22:Ph2();break;
+ case 23:Ph2();break;
+ case 24:Ph2();break;
+ case 25:Ph2();break;
+ case 26:Ph2();break;
+ case 27:Ph2();break;
+ case 28:Ph2();break;
+ case 29:Ph2();break;
+ case 30:Ph2();break;
+ case 31:Ph2();break;
+ case 32:Ph3();break;
+ case 33:Ph3();break;
+ case 34:Ph3();break;
+ case 35:Ph3();break;
+ case 36:Ph3();break;
+ case 37:Ph3();break;
+ case 38:Ph3();break;
+ case 39:Ph3();break;
+ case 40:Ph3();break;
+ case 41:Ph3();break;
+ case 42:Ph3();break;
+ case 43:Ph3();break;
+ case 44:Ph3();break;
+ case 45:Ph3();break;
+ case 46:Ph3();break;
+ case 47:Ph3();break;
+ case 48:Ph4();break;
+ case 49:Ph4();break;
+ case 50:Ph4();break;
+ case 51:Ph4();break;
+ case 52:Ph4();break;
+ case 53:Ph4();break;
+ case 54:Ph4();break;
+ case 55:Ph4();break;
+ case 56:Ph4();break;
+ case 57:Ph4();break;
+ case 58:Ph4();break;
+ case 59:Ph4();break;
+ case 60:Ph4();break;
+ case 61:Ph4();break;
+ case 62:Ph4();break;
+ case 63:Ph4();break;
default:break;
}
-
+ /*if(wheel.getYay()==1)
+ {
+ pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ }*/
if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
{
RPM();
VelocityLoop();
+ slowloop++;
+ if(slowloop>rpm)
+ {
+ ReadKType();
+ slowloop=0;
+ }
}
}
while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
{
GetChar();
- StateB = (800+wheel.getPulses()+StateA+AdjCW)%16;
+ StateB = (enc+wheel.getPulses()+StateA+AdjCW)%s;
//pc.printf("StateA= %i\r", StateA);
- //pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ //pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
switch(StateB)
{
@@ -177,91 +298,263 @@
case 1:Ph1();break;
case 2:Ph1();break;
case 3:Ph1();break;
- case 4:Ph2();break;
- case 5:Ph2();break;
- case 6:Ph2();break;
- case 7:Ph2();break;
- case 8:Ph3();break;
- case 9:Ph3();break;
- case 10:Ph3();break;
- case 11:Ph3();break;
- case 12:Ph4();break;
- case 13:Ph4();break;
- case 14:Ph4();break;
- case 15:Ph4();break;
+ case 4:Ph1();break;
+ case 5:Ph1();break;
+ case 6:Ph1();break;
+ case 7:Ph1();break;
+ case 8:Ph1();break;
+ case 9:Ph1();break;
+ case 10:Ph1();break;
+ case 11:Ph1();break;
+ case 12:Ph1();break;
+ case 13:Ph1();break;
+ case 14:Ph1();break;
+ case 15:Ph1();break;
+ case 16:Ph2();break;
+ case 17:Ph2();break;
+ case 18:Ph2();break;
+ case 19:Ph2();break;
+ case 20:Ph2();break;
+ case 21:Ph2();break;
+ case 22:Ph2();break;
+ case 23:Ph2();break;
+ case 24:Ph2();break;
+ case 25:Ph2();break;
+ case 26:Ph2();break;
+ case 27:Ph2();break;
+ case 28:Ph2();break;
+ case 29:Ph2();break;
+ case 30:Ph2();break;
+ case 31:Ph2();break;
+ case 32:Ph3();break;
+ case 33:Ph3();break;
+ case 34:Ph3();break;
+ case 35:Ph3();break;
+ case 36:Ph3();break;
+ case 37:Ph3();break;
+ case 38:Ph3();break;
+ case 39:Ph3();break;
+ case 40:Ph3();break;
+ case 41:Ph3();break;
+ case 42:Ph3();break;
+ case 43:Ph3();break;
+ case 44:Ph3();break;
+ case 45:Ph3();break;
+ case 46:Ph3();break;
+ case 47:Ph3();break;
+ case 48:Ph4();break;
+ case 49:Ph4();break;
+ case 50:Ph4();break;
+ case 51:Ph4();break;
+ case 52:Ph4();break;
+ case 53:Ph4();break;
+ case 54:Ph4();break;
+ case 55:Ph4();break;
+ case 56:Ph4();break;
+ case 57:Ph4();break;
+ case 58:Ph4();break;
+ case 59:Ph4();break;
+ case 60:Ph4();break;
+ case 61:Ph4();break;
+ case 62:Ph4();break;
+ case 63:Ph4();break;
default:break;
}
-
+ /*if(wheel.getYay()==1)
+ {
+ pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ }*/
if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
{
RPM();
VelocityLoop();
+ slowloop++;
+ if(slowloop>rpm)
+ {
+ ReadKType();
+ slowloop=0;
+ }
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
{
GetChar();
//StateB = (800+wheel.getPulses())%16;
- StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
+ StateB = (wheel.getPulses()+StateA+AdjACW)%s;
//pc.printf("StateA= %i\r", StateA);
- //pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ //pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
switch(StateB)
{
- case 15:Ph4();break;
- case 14:Ph4();break;
- case 13:Ph4();break;
- case 12:Ph4();break;
- case 11:Ph3();break;
- case 10:Ph3();break;
- case 9:Ph3();break;
- case 8:Ph3();break;
- case 7:Ph2();break;
- case 6:Ph2();break;
- case 5:Ph2();break;
- case 4:Ph2();break;
+ case 63:Ph4();break;
+ case 62:Ph4();break;
+ case 61:Ph4();break;
+ case 60:Ph4();break;
+ case 59:Ph4();break;
+ case 58:Ph4();break;
+ case 57:Ph4();break;
+ case 56:Ph4();break;
+ case 55:Ph4();break;
+ case 54:Ph4();break;
+ case 53:Ph4();break;
+ case 52:Ph4();break;
+ case 51:Ph4();break;
+ case 50:Ph4();break;
+ case 49:Ph4();break;
+ case 48:Ph4();break;
+ case 47:Ph3();break;
+ case 46:Ph3();break;
+ case 45:Ph3();break;
+ case 44:Ph3();break;
+ case 43:Ph3();break;
+ case 42:Ph3();break;
+ case 41:Ph3();break;
+ case 40:Ph3();break;
+ case 39:Ph3();break;
+ case 38:Ph3();break;
+ case 37:Ph3();break;
+ case 36:Ph3();break;
+ case 35:Ph3();break;
+ case 34:Ph3();break;
+ case 33:Ph3();break;
+ case 32:Ph3();break;
+ case 31:Ph2();break;
+ case 30:Ph2();break;
+ case 29:Ph2();break;
+ case 28:Ph2();break;
+ case 27:Ph2();break;
+ case 26:Ph2();break;
+ case 25:Ph2();break;
+ case 24:Ph2();break;
+ case 23:Ph2();break;
+ case 22:Ph2();break;
+ case 21:Ph2();break;
+ case 20:Ph2();break;
+ case 19:Ph2();break;
+ case 18:Ph2();break;
+ case 17:Ph2();break;
+ case 16:Ph2();break;
+ case 15:Ph1();break;
+ case 14:Ph1();break;
+ case 13:Ph1();break;
+ case 12:Ph1();break;
+ case 11:Ph1();break;
+ case 10:Ph1();break;
+ case 9:Ph1();break;
+ case 8:Ph1();break;
+ case 7:Ph1();break;
+ case 6:Ph1();break;
+ case 5:Ph1();break;
+ case 4:Ph1();break;
case 3:Ph1();break;
case 2:Ph1();break;
case 1:Ph1();break;
case 0:Ph1();break;
default:break;
}
-
+ /*if(wheel.getYay()==1)
+ {
+ pc.printf("3 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ }*/
if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
{
RPM();
VelocityLoop();
+ slowloop++;
+ if(slowloop>rpm)
+ {
+ ReadKType();
+ slowloop=0;
+ }
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
{
GetChar();
- StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
+ StateB = (enc+wheel.getPulses()+StateA+AdjACW)%s;
//pc.printf("StateA= %i\r", StateA);
- //pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ //pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
switch(StateB)
{
- case 15:Ph4();break;
- case 14:Ph4();break;
- case 13:Ph4();break;
- case 12:Ph4();break;
- case 11:Ph3();break;
- case 10:Ph3();break;
- case 9:Ph3();break;
- case 8:Ph3();break;
- case 7:Ph2();break;
- case 6:Ph2();break;
- case 5:Ph2();break;
- case 4:Ph2();break;
+ case 63:Ph4();break;
+ case 62:Ph4();break;
+ case 61:Ph4();break;
+ case 60:Ph4();break;
+ case 59:Ph4();break;
+ case 58:Ph4();break;
+ case 57:Ph4();break;
+ case 56:Ph4();break;
+ case 55:Ph4();break;
+ case 54:Ph4();break;
+ case 53:Ph4();break;
+ case 52:Ph4();break;
+ case 51:Ph4();break;
+ case 50:Ph4();break;
+ case 49:Ph4();break;
+ case 48:Ph4();break;
+ case 47:Ph3();break;
+ case 46:Ph3();break;
+ case 45:Ph3();break;
+ case 44:Ph3();break;
+ case 43:Ph3();break;
+ case 42:Ph3();break;
+ case 41:Ph3();break;
+ case 40:Ph3();break;
+ case 39:Ph3();break;
+ case 38:Ph3();break;
+ case 37:Ph3();break;
+ case 36:Ph3();break;
+ case 35:Ph3();break;
+ case 34:Ph3();break;
+ case 33:Ph3();break;
+ case 32:Ph3();break;
+ case 31:Ph2();break;
+ case 30:Ph2();break;
+ case 29:Ph2();break;
+ case 28:Ph2();break;
+ case 27:Ph2();break;
+ case 26:Ph2();break;
+ case 25:Ph2();break;
+ case 24:Ph2();break;
+ case 23:Ph2();break;
+ case 22:Ph2();break;
+ case 21:Ph2();break;
+ case 20:Ph2();break;
+ case 19:Ph2();break;
+ case 18:Ph2();break;
+ case 17:Ph2();break;
+ case 16:Ph2();break;
+ case 15:Ph1();break;
+ case 14:Ph1();break;
+ case 13:Ph1();break;
+ case 12:Ph1();break;
+ case 11:Ph1();break;
+ case 10:Ph1();break;
+ case 9:Ph1();break;
+ case 8:Ph1();break;
+ case 7:Ph1();break;
+ case 6:Ph1();break;
+ case 5:Ph1();break;
+ case 4:Ph1();break;
case 3:Ph1();break;
case 2:Ph1();break;
case 1:Ph1();break;
case 0:Ph1();break;
- default:break;
+ default:break;
}
- if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
+ /*if(wheel.getYay()==1)
+ {
+ pc.printf("4 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ }*/
+ if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
{
RPM();
VelocityLoop();
+ slowloop++;
+ if(slowloop>rpm)
+ {
+ ReadKType();
+ slowloop=0;
+ }
}
}
while(temp>(T-1))
@@ -284,22 +577,55 @@
}
void StepCW(void) //Square wave switching
{
- Ph1();
+ /*Ph1();
wait(x);
- //Ph12();
- //wait(y);
+ //pc.printf("1 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph12();
+ wait(y);
+ //pc.printf("12 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
Ph2();
wait(x);
- //Ph23();
- //wait(y);
+ //pc.printf("2 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph23();
+ wait(y);
+ //pc.printf("23 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
Ph3();
wait(x);
- //Ph34();
- //wait(y);
+ //pc.printf("3 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph34();
+ wait(y);
+ //pc.printf("34 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph4();
+ wait(x);
+ //pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph41();
+ wait(y);
+ //pc.printf("41 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ */
Ph4();
wait(x);
- //Ph41();
- //wait(y);
+ //pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph34();
+ wait(y);
+ //pc.printf("34 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph3();
+ wait(x);
+ //pc.printf("3 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph23();
+ wait(y);
+ //pc.printf("23 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph2();
+ wait(x);
+ //pc.printf("2 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph12();
+ wait(y);
+ //pc.printf("12 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph1();
+ wait(x);
+ //pc.printf("1 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
+ Ph41();
+ wait(y);
+ //pc.printf("41 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
}
void Ph0(void)
@@ -400,6 +726,7 @@
UnUsedPhase1=0;
UnUsedPhase2=0;
wheel.ResetYay();
+ wheel.QEI::reset();
}
void GetChar (void) //read keyboard strikes with terraterm
@@ -408,16 +735,16 @@
c = pc.getc();
if(c == 'z') //turn on led1 causes CW operation
{
- AdjCW = 0;
- AdjACW = 10;
+ //AdjCW = 0;
+ //AdjACW = 10;
led1 = !led1;
led2 = 0;
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
if(c == 'x') //turn on led2 causes ACW operation
{
- AdjCW = 0;
- AdjACW = 10;
+ //AdjCW = 0;
+ //AdjACW = 10;
led1 = 0;
led2 = !led2 ;
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
@@ -426,9 +753,9 @@
{
//duty = duty + 0.0001;
SetPoint=SetPoint+5;
- if (SetPoint >2200)
+ if (SetPoint >3000)
{
- SetPoint = 2200;
+ SetPoint = 3000;
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
@@ -442,10 +769,50 @@
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
+ if(c == 'w') //Increases setpoint used in Velocity loop
+ {
+ duty = duty + 0.1;
+ /*SetPoint=SetPoint+5;
+ if (SetPoint >2200)
+ {
+ SetPoint = 2200;
+ }*/
+ pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
+ }
+ if(c == 's') //Decreases setpoint used in Velocity loop
+ {
+ duty = duty - 0.1;
+ /*SetPoint=SetPoint-5;
+ if (SetPoint <50)
+ {
+ SetPoint = 50;
+ }*/
+ pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
+ }
+ if(c == 'e') //Increases setpoint used in Velocity loop
+ {
+ AdjDiff = AdjDiff * 10;
+ /*SetPoint=SetPoint+5;
+ if (SetPoint >2200)
+ {
+ SetPoint = 2200;
+ }*/
+ pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
+ }
+ if(c == 'd') //Decreases setpoint used in Velocity loop
+ {
+ AdjDiff = AdjDiff / 10;
+ /*SetPoint=SetPoint-5;
+ if (SetPoint <50)
+ {
+ SetPoint = 50;
+ }*/
+ pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
+ }
if(c== 'o')
{
AdjCW = AdjCW+1;
- if (AdjCW >15)
+ if (AdjCW >(s-1))
{
AdjCW = 0;
}
@@ -456,14 +823,14 @@
AdjCW = AdjCW-1;
if (AdjCW <0)
{
- AdjCW = 15;
+ AdjCW = (s-1);
}
pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
}
if(c== 'p')
{
AdjACW = AdjACW+1;
- if (AdjACW >15)
+ if (AdjACW >(s-1))
{
AdjACW = 0;
}
@@ -474,73 +841,182 @@
AdjACW = AdjACW-1;
if (AdjACW <0)
{
- AdjACW = 15;
+ AdjACW = (s-1);
}
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);
+ pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
}
if(c=='t')
{
pc.printf("%.0f C\n\r",temp);
}
+ if(c=='y')
+ {
+ pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ }
}
}
void RPM (void)
{
- wheel.ResetWhoop(); //PulseCount2_==800, whoop_=1;
- TimePerClick = (t.read_us()); //read timer in microseconds
- t.reset(); //reset timer
- TimePerRev = TimePerClick * (800/z); //z = 800 (PulseCount2_==800)
- TimePerRev = TimePerRev / 1000; //
- RPS = 10000000 / TimePerRev;
- rpm = (RPS * 60)/10000;
- //Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
- ReadKType();
- if(rpm < 10)
+ wheel.ResetWhoop(); //PulseCount2_==400 x 4, x4 encoding, whoop_=1;
+ TimePerClick = (t.read_us()); //read timer in microseconds
+ t.reset(); //reset timer
+ TimePerRev = enc * TimePerClick / z; //z = 400 (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
+ //Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
+ //ReadKType();
+ if(rpm < 300)
+ {
+ AdjCW = 57;
+ AdjACW = 12;
+ }
+ if(rpm > 299 && rpm <400)
+ {
+ AdjCW = 62;
+ AdjACW = 11;
+ }
+ if(rpm > 399 and rpm < 525)
+ {
+ AdjCW = 63;
+ AdjACW = 10;
+ }
+ if(rpm > 524 and rpm < 625)
{
AdjCW = 0;
- AdjACW = 10;
+ AdjACW = 9;
+ }
+ if(rpm > 624 and rpm < 725)
+ {
+ AdjCW = 1;
+ AdjACW = 8;
}
- if(rpm > 9 and rpm < 124)
+ if(rpm > 724 and rpm < 850)
+ {
+ AdjCW = 2;
+ AdjACW = 7;
+ }
+ if(rpm > 849 and rpm < 975)
{
AdjCW = 3;
- AdjACW = 7;
+ AdjACW = 6;
}
- if(rpm > 125 and rpm < 319)
+ if(rpm > 974 and rpm < 1150)
{
AdjCW = 4;
- AdjACW = 6;
- }
- if(rpm > 320 and rpm < 399)
+ AdjACW = 5;
+ }
+ if(rpm > 1149 and rpm < 1375)
{
AdjCW = 5;
- AdjACW = 6;
+ AdjACW = 4;
+ }
+ if(rpm > 1374 and rpm < 1725)
+ {
+ AdjCW = 6;
+ AdjACW = 3;
+ }
+ if(rpm > 1724 and rpm < 2150)
+ {
+ AdjCW = 7;
+ AdjACW = 2;
}
- if(rpm > 400)
+ if(rpm > 2149 and rpm < 2525)
+ {
+ AdjCW = 8;
+ AdjACW = 1;
+ }
+ if(rpm > 2524 and rpm < 2600)
+ {
+ AdjCW = 9;
+ AdjACW = 0;
+ }
+ if(rpm > 2599 and rpm < 2650)
{
- AdjCW = 5;
- AdjACW = 5;
+ AdjCW = 10;
+ AdjACW = 63;
+ }
+ if(rpm > 2649 and rpm < 2700)
+ {
+ AdjCW = 11;
+ AdjACW = 62;
+ }
+ if(rpm > 2699 and rpm < 2750)
+ {
+ AdjCW = 12;
+ AdjACW = 61;
}
-
- //pc.printf("rpm = %i\r", rpm);
+ if(rpm > 2749 and rpm < 2825)
+ {
+ AdjCW = 13;
+ AdjACW = 60;
+ }
+ if(rpm > 2824 and rpm < 2875)
+ {
+ AdjCW = 14;
+ AdjACW = 59;
+ }
+ if(rpm > 2874)
+ {
+ AdjCW = 15;
+ AdjACW = 58;
+ }
+ //pc.printf("RPS = %i \r", RPS);
//pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
}
void VelocityLoop (void)
{
- diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
- duty = duty + (diff*0.001); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. 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
- {
+ diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
+ duty = duty + (diff*AdjDiff); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ /*if(duty > 0.8)
+ {
+ duty = duty + (diff*0.000001); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ }
+ if(0.5 < duty < 0.8)
+ {
+ duty = duty + (diff*0.005); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ }
+ if(duty < 0.5)
+ {
+ duty = duty + (diff*0.001); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ }
+ /*if(0 < rpm < 500)
+ {
+ duty = duty + (diff*0.0001); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ }
+ if(500 < rpm < 1000)
+ {
+ duty = duty + (diff*0.00008); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ }
+ if(1000 < rpm < 1500)
+ {
+ duty = duty + (diff*0.00006); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ }
+ if(1500 < rpm < 2000)
+ {
+ duty = duty + (diff*0.00004); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ }
+ if(2000 < rpm < 2500)
+ {
+ duty = duty + (diff*0.00002); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
+ }
+ if(2500 < rpm < 3000)
+ {
+ duty = duty + (diff*0.00001); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. 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.32490) //3A min duty 0.96, 6.5A min duty 0.4
+ if (duty <0.1) //3A min duty 0.96, 6.5A min duty 0.4
{
- duty = 0.32490;
+ duty = 0.1;
}
//pc.printf("%i, %.5f, %i, %i, %i \r", SetPoint, duty, AdjCW, AdjACW, rpm); //SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
}
@@ -551,9 +1027,9 @@
int Readout = 0;
cs1 = 0;
- //SerialClock = 0; //set clock to 0
- //wait_ms(1);
- SerialClock = 1; //clock once to set to the 13 bit temp data
+ SerialClock = 0; //set clock to 0
+ wait_ms(1);
+ SerialClock = 1; //clock once to set to the 13 bit temp data
wait_ms(0.1);
SerialClock = 0;
wait_ms(0.1);