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:
- 20:dca9f4c12fe3
- Parent:
- 19:d9ba2f225f39
- Child:
- 21:b831f68ce5ed
--- a/main.cpp Fri Apr 05 10:31:29 2019 +0000
+++ b/main.cpp Thu Apr 11 14:53:32 2019 +0000
@@ -1,7 +1,18 @@
#include "mbed.h"
#include "QEI.h"
-void Initialisation (void); //These voids are written after the main. They must be listed here too (functional prototypes).
+/* Code written by Simon Truelove (completed 5/4/2019) to control Switched Reluctance Motor (SRM) modified from a
+QSH6018-65-28-210 stepper motor. The position of the motor is monitored using a rotary encoder 102-1307-ND using QEI.h.
+The code uses the pulses from the encoder to increment the values StateA and StateB to control switching of the phases needed
+to generate rotation via a 64 case state machine. Upon start up the motor will step anti clockwise by switching each phase
+on in sequence then it will complete 2 revolutions to find the encoder index and set StateA which is used for calculating
+StateB. StateB controls 4 identical state machines, however state B is calculated slightly differently within each one
+depending on the direction of rotation required and the direction the motor had turned previously. LED's are switched on and
+off and these LED's control which of the 4 state machines is used.
+"Its flow"
+*/
+
+void Initialisation (void); //These function prototypes are written after the main. They must be listed here too
void StepACW(void);
void Ph0(void);
void Ph1(void);
@@ -23,65 +34,51 @@
Timer t; //timer used in RPM
-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
-
-//DigitalIn Button1 (p11); //not used
-//DigitalIn Button2 (p12); //not used
-
-DigitalOut led1(LED1); //LEDs used to as very basic memmory for controlling the state machines
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
-DigitalOut SerialClock (p12); //ReadKType
-DigitalIn DOut (p13); //ReadKType
-DigitalOut cs1 (p14); //ReadKType
+DigitalOut led1 (LED1); //LEDs used to as very basic memmory for controlling the state machines
+DigitalOut led2 (LED2);
+DigitalOut led3 (LED3);
+DigitalOut led4 (LED4);
+DigitalOut SerialClock (p12); //ReadKType SPI
+DigitalIn DOut (p13); //ReadKType SPI
+DigitalOut cs1 (p14); //Chip Select SPI
+DigitalOut UnUsedPhase1 (p21);
+PwmOut Phase1 (p22); //Pin set up - PWM to enable speed control
+PwmOut Phase2 (p23);
+PwmOut Phase3 (p24);
+PwmOut Phase4 (p25);
+DigitalOut UnUsedPhase2 (p26);
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 = 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 AdjCW = 57; //CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
+int AdjACW = 12; //ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
+int CW = 57; //Value used to start motor from stationary
+int ACW = 12; //Value used to start motor from stationary
int TimePerClick = 0; //for calc of RPM
-int SetPoint = 1000; //for adjusting the speed
+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;
-int l = 0;
-int m = 0;
-int n = 0;
-int s = enc/50;
-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;
-int StartUp = 1;
+int s = enc/50; //s=number of cases in state machine (64)
+int T = 80; //Thermostat protection limit degrees centigrade
+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. //Motor temp limit
+int slowloop = 0; //Counter used to read temp at a slower rate
char c; //keyboard cotrol GetChar
-float RPS = 0; //for calc of RPM
-float rpm = 0; //for calc of RPM
+float rps = 0; //for calc of RPM
+float rpm = 0; //for calc of RPM
float duty = 1;
float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
-float AdjDiff = 0.001;
-float p = 0.000014;
-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 gain = 0.001; //Velocity loop
+float p = 0.000014; //PWM period
+float x=0.1; //Used in StepACW x=time of square wave when 1 phase energised,
+float TimePerRev = 0; //for calc of RPM
+float y=0.04; //Used in Step ACWy=time of square wave when 2 phases energised
float temp = 0; //ReadKType
int main(void)
{
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.
+ Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
Phase2.period(p);
Phase3.period(p);
Phase4.period(p);
@@ -90,18 +87,13 @@
SerialClock = 0;
StepACW();
Initialisation();
-
-/*while(1)
-{
-}
-*/
-while(wheel.getRevolutions()<2) //Index Calibration
- {
+ while(wheel.getRevolutions()<2) //Index Calibration
+ {
StateA = (wheel.getPulses()+25)%s;
switch(StateA)
{
- case 0:Ph1();break;//;pc.printf("1 Pulses= %i\n\r", wheel.getPulses());break;
+ case 0:Ph1();break;
case 1:Ph1();break;
case 2:Ph1();break;
case 3:Ph1();break;
@@ -117,7 +109,7 @@
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 16:Ph2();break;
case 17:Ph2();break;
case 18:Ph2();break;
case 19:Ph2();break;
@@ -133,7 +125,7 @@
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 32:Ph3();break;
case 33:Ph3();break;
case 34:Ph3();break;
case 35:Ph3();break;
@@ -149,7 +141,7 @@
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 48:Ph4();break;
case 49:Ph4();break;
case 50:Ph4();break;
case 51:Ph4();break;
@@ -167,7 +159,7 @@
case 63:Ph4();break;
default:break;
}
- if(wheel.getYay()==1) //PulseCount_==1, yay_=1;
+ if(wheel.getYay()==1) //PulseCount_==1, yay_=1 used to increment the StateA count
{
StateA++;
wheel.ResetYay();
@@ -182,36 +174,19 @@
{
while((led1 == 0) && (led2 == 0)) //If no command to operate
{
- //Aout = 0;
- duty = 0.3;
- rpm = 0;
- AdjCW = CW;
- AdjACW = ACW;
- Ph0();
- GetChar();
- ReadKType();
- //VelocityLoop();
- StateB = (wheel.getPulses()+StateA)%s; //wheel.getPulses()%1(s-1);
- //wait(0.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());
+ duty = 0.3; //Duty reduced to low value to ensure ramp up of speed
+ rpm = 0; //RPM function not being triggered due to no rotation. RPM set to 0
+ AdjCW = CW; //Reset to correct value for start up
+ AdjACW = ACW; //Reset to correct value for start up
+ Ph0(); //turn off all phases
+ GetChar(); //read keyboard strikes
+ ReadKType(); //Temperature measurement
+ StateB = (wheel.getPulses()+StateA)%s; //calculation for stateB
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
{
- GetChar();
- //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
- //Phase2.period(p);
- //Phase3.period(p);
- //Phase4.period(p);
- 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\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+ GetChar(); //read keyboard strikes
+ StateB = (wheel.getPulses()+StateA+AdjCW)%s; //calculation for stateB
switch(StateB)
{
@@ -281,20 +256,16 @@
case 63:Ph0();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_==3200, whoop_=1;
+
+ if(wheel.getWhoop()==1) //PulseCount2_==3200 (this is 1 rotation), whoop_=1;
{
- RPM();
- VelocityLoop();
- slowloop++;
- if(slowloop>(0.01*rpm))
+ RPM(); //calculate RPM
+ VelocityLoop(); //Adjust velocity
+ slowloop++; //increment slowloop
+ if(slowloop>(0.01*rpm)) //Reads temperature at a constant time interval
{
- ReadKType();
- slowloop=0;
- //pc.printf("%i, %.4f\n\r", rpm, duty);
+ ReadKType(); //Read Temp
+ slowloop=0; //Reset slowloop
}
}
}
@@ -302,14 +273,8 @@
while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
{
GetChar();
- //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
- //Phase2.period(p);
- //Phase3.period(p);
- //Phase4.period(p);
StateB = (enc+wheel.getPulses()+StateA+AdjCW)%s;
- //pc.printf("StateA= %i\r", StateA);
- //pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
-
+
switch(StateB)
{
case 0:Ph1();break;
@@ -378,11 +343,8 @@
case 63:Ph0();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_==3200, whoop_=1;
+
+ if(wheel.getWhoop()==1)
{
RPM();
VelocityLoop();
@@ -391,21 +353,14 @@
{
ReadKType();
slowloop=0;
- //pc.printf("%i, %.4f\n\r", rpm, duty);
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
{
GetChar();
- //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
- //Phase2.period(p);
- //Phase3.period(p);
- //Phase4.period(p);
- //StateB = (800+wheel.getPulses())%16;
StateB = (wheel.getPulses()+StateA+AdjACW)%s;
- //pc.printf("StateA= %i\r", StateA);
- //pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+
switch(StateB)
{
case 63:Ph4();break;
@@ -473,12 +428,8 @@
case 1:Ph1();break;
case 0:Ph0();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_==3200, whoop_=1;
+ }
+ if(wheel.getWhoop()==1)
{
RPM();
VelocityLoop();
@@ -487,20 +438,14 @@
{
ReadKType();
slowloop=0;
- //pc.printf("%i, %.4f\n\r", rpm, duty);
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
{
GetChar();
- //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
- //Phase2.period(p);
- //Phase3.period(p);
- //Phase4.period(p);
StateB = (enc+wheel.getPulses()+StateA+AdjACW)%s;
- //pc.printf("StateA= %i\r", StateA);
- //pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
+
switch(StateB)
{
case 63:Ph4();break;
@@ -569,11 +514,8 @@
case 0:Ph0();break;
default:break;
}
- /*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_==3200, whoop_=1;
+
+ if(wheel.getWhoop()==1)
{
RPM();
VelocityLoop();
@@ -582,25 +524,24 @@
{
ReadKType();
slowloop=0;
- //pc.printf("%i, %.4f\n\r", rpm, duty);
}
}
}
- while(temp>(T-1))
+ while(temp>(T-1)) //If Temp exceeds T the motor will not operate
{
Initialisation();
pc.printf("Motor Over Temp\n\r");
- while(1)
+ while(1)
{
- ReadKType();
- pc.printf("%f\r",temp);
- wait(1);
- if(temp<T-20)
- {
- pc.printf("Motor Back Online\n\r");
- break;
+ ReadKType(); //Read motor Temp
+ pc.printf("%f\r",temp); //Display current Temp
+ wait(1);
+ if(temp<T-20) //Reset when Temp 20 degrees C below thermostat trip value
+ {
+ pc.printf("Motor Back Online\n\r");
+ break;
}
- }
+ }
}
}
}
@@ -632,92 +573,79 @@
//pc.printf("41 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
}
-void Ph0(void)
+void Ph0(void) //Turn off all Phases
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(0);
Phase4.write(0);
- //wait(x);
- //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
}
-void Ph1(void)
+void Ph1(void) //Turn on Phase 1
{
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)
+void Ph12 (void) //Turn on Phase 1 and 2
{
Phase1.write(duty);
Phase2.write(duty);
Phase3.write(0);
Phase4.write(0);
- //wait(y);
}
-void Ph2(void)
+void Ph2(void) //Turn on Phase 2
{
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)
+void Ph23 (void) //Turn on Phase 2 and 3
{
Phase1.write(0);
Phase2.write(duty);
Phase3.write(duty);
Phase4.write(0);
- //wait(y);
}
-void Ph3(void)
+void Ph3(void) //Turn on Phase 3
{
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)
+
+void Ph34 (void) //Turn on Phase 3 and 4
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(duty);
Phase4.write(duty);
- //wait(y);
}
-void Ph4(void)
+void Ph4(void) //Turn on Phase 4
{
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)
+void Ph41 (void) //Turn on Phase 4 and 1
{
Phase1.write(duty);
Phase2.write(0);
Phase3.write(0);
Phase4.write(duty);
- //wait(y);
}
-void Initialisation (void) //Turn everything off
+void Initialisation (void) //Turn everything off
{
Phase1.write(0);
Phase2.write(0);
@@ -733,171 +661,37 @@
wheel.QEI::reset();
}
-void GetChar (void) //read keyboard strikes with terraterm
+void GetChar (void) //read keyboard strikes with terraterm
{ if (pc.readable())
{
c = pc.getc();
- if(c ==' ')
- {
-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 == '7')
- {
- pc.printf("TimePerClick = %i, TimePerRev = %.5f, RPS = %.5f, rpm = %.5f, \n\r", TimePerClick, TimePerRev, RPS, rpm);
- }
- if(c == '9')
- {
- pc.printf("StateA = %i, StateB = %i, \n\r", StateA, StateB);
- }
- 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') //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') //Increases setpoint used in Velocity loop
- {
- SetPoint=SetPoint+5;
- if (SetPoint >3000)
- {
- SetPoint = 3000;
- }
- pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
- }
- if(c == 'a') //Decreases setpoint used in Velocity loop
- {
- SetPoint=SetPoint-5;
- if (SetPoint <0)
- {
- SetPoint = 0;
- }
- 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.01;
- 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.01;
- 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
+ switch(c)
{
- AdjDiff = AdjDiff * 10;
- 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;
- pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
- }
- if(c== 'o')
- {
- AdjCW = AdjCW+1;
- if (AdjCW >(s-1))
- {
- 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 = (s-1);
- }
- pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
- }
- if(c== 'p')
- {
- AdjACW = AdjACW+1;
- if (AdjACW >(s-1))
- {
- 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 = (s-1);
- }
- pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
- }
- if(c== 'i')
- {
- n = n+1;
- /*if (n > 63)
- {
- n = 0;
- }*/
- pc.printf("%i, %.5f, %i, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n);
- }
- if(c== 'j')
- {
- n = n-1;
- /*if (n <0)
- {
- n = 63;
- }*/
- pc.printf("%i, %.5f, %i, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n);
- }
- if(c== 'r')
- {
- p = p+0.0000001;
- pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);
- }
- if(c== 'f')
- {
- p = p-0.0000001;
- pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);
- }
- if(c=='0')
- {
- pc.printf("%i, %.5f, %i, %i, %.1f, %i, %f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n, AdjDiff, p);
- }
- if(c=='t')
- {
- pc.printf("%.0f C\n\r",temp);
- }
- if(c=='y')
- {
- pc.printf("State A = %i, StateB = %i, Pulses= %i, Revs= %i \n\r", StateA, StateB, wheel.getPulses(), wheel.getRevolutions());
+ case ' ': pc.printf(" 0 = Phase Mapping\n\rq = setpoint+, w = duty+, e = gain+, t = temp, y = states, o = AdjCW+, p = AdjACW+,\n\r a = setpoint-, s = duty-, d = gain-, k = AdjCW-, l = AdjACW-,\n\r z = CW, x = ACW\n\r");break;
+ case '1': Ph1();break; //Check Phase 1
+ case '2': Ph2();break; //Check Phase 2
+ case '3': Ph3();break; //Check Phase 3
+ case '4': Ph4();break; //Check Phase 4
+ case '5': Ph0();break; //Turn off all Phases
+ case '6': StepACW();break; //Step ACW
+ case '7': pc.printf("TimePerClick = %i, TimePerRev = %.5f, rps = %.5f, rpm = %.1f, \n\r", TimePerClick, TimePerRev, rps, rpm);break; //Print rpm related data
+ case '9': pc.printf("State A = %i, StateB = %i, Pulses= %i, Revs= %i \n\r", StateA, StateB, wheel.getPulses(), wheel.getRevolutions());break; //Print data relating to position
+ case '0': pc.printf("%i, %.5f, %i, %i, %.1f, %f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, gain, p);break; //Print general data
+ case 'q': SetPoint=SetPoint+5;if (SetPoint >3000){SetPoint = 3000;}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Increases setpoint used in Velocity loop
+ case 'a': SetPoint=SetPoint-5;if (SetPoint <0){SetPoint = 0;}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Decreases setpoint used in Velocity loop
+ case 'w': duty = duty + 0.01;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Increases duty used in Velocity loop
+ case 's': duty = duty - 0.01;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Decreases duty used in Velocity loop
+ case 'e': gain = gain * 10;pc.printf("%i, %.5f, %i, %i, %.1f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, gain);break; //Increases gain used in Velocity loop
+ case 'd': gain = gain / 10;pc.printf("%i, %.5f, %i, %i, %.1f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, gain);break; //Decreases gain used in Velocity loop
+ case 'r': p = p+0.0000001;pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);break; //Increasees PWM period
+ case 'f': p = p-0.0000001;pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);break; //Decreasees PWM period
+ case 't': pc.printf("%.0f C\n\r",temp);break; //Print current Temp
+ case 'o': AdjCW = AdjCW+1;if (AdjCW >(s-1)){AdjCW = 0;}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //AdjCW+
+ case 'k': AdjCW = AdjCW-1;if (AdjCW <0){AdjCW = (s-1);}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //AdjCW-
+ case 'p': AdjACW = AdjACW+1;if (AdjACW >(s-1)){AdjACW = 0;}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //AdjACW+
+ case 'l': AdjACW = AdjACW-1;if (AdjACW <0){AdjACW = (s-1);}pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //AdjACW-
+ case 'z': led1 = !led1;led2 = 0;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Run motor CW
+ case 'x': led1 = 0;led2 = !led2;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Run motor ACW
}
}
}
@@ -907,240 +701,281 @@
wheel.ResetWhoop(); //Whoop = 1 x per rev
TimePerClick = (t.read_us()); //read timer in microseconds
t.reset(); //reset timer
- TimePerRev = TimePerClick; //enc * TimePerClick / z; //enc = 3200, z = 3200 (PulseCount2_==1600)
+ TimePerRev = TimePerClick; //Convert from int to float TimePerClick = int, TimePerRev = float
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(duty < 1)
- {
- if(rpm < 65)
- {
- AdjCW = (57);
- AdjACW = (14);
- }
- if(rpm > 64 and rpm <75)
- {
- AdjCW = (57);
- AdjACW = (12);
- }
- if(rpm > 74 and rpm < 90)
- {
- AdjCW = (60);
- AdjACW = (9);
- }
- if(rpm > 89 and rpm < 200)
- {
- AdjCW = (60);
- AdjACW = (9);
- }
- if(rpm > 199 and rpm < 300)
- {
- AdjCW = (61);
- AdjACW = (8);
- }
- if(rpm > 299 and rpm <400)
- {
- AdjCW = (62);
- AdjACW = (7);
- }
- if(rpm > 399 and rpm < 525)
- {
- AdjCW = (63);
- AdjACW = (6);
- }
- if(rpm > 524 and rpm < 625)
- {
- AdjCW = (0);
- AdjACW = (5);
- }
- if(rpm > 624 and rpm < 725)
- {
- AdjCW = (1);
- AdjACW = (4);
- }
- if(rpm > 724 and rpm < 850)
- {
- AdjCW = (2);
- AdjACW = (3);
- }
- if(rpm > 849 and rpm < 975)
- {
- AdjCW = (3);
- AdjACW = (2);
- }
- if(rpm > 974 and rpm < 1150)
- {
- AdjCW = (4);
- AdjACW = (1);
- }
- if(rpm > 1149 and rpm < 1375)
- {
- AdjCW = (5);
- AdjACW = (0);
- }
- if(rpm > 1374 and rpm < 1725)
- {
- AdjCW = (6);
- AdjACW = (63);
- }
- if(rpm > 1724 and rpm < 2150)
- {
- AdjCW = (7);
- AdjACW = (62);
- }
- if(rpm > 2149 and rpm < 2525)
- {
- AdjCW = (8);
- AdjACW = (61);
- }
- if(rpm > 2524 and rpm < 2600)
- {
- AdjCW = (9);
- AdjACW = (60);
- }
- if(rpm > 2599 and rpm < 2650)
- {
- AdjCW = (10);
- AdjACW = (59);
- }
- if(rpm > 2649 and rpm < 2700)
- {
- AdjCW = (11);
- AdjACW = (58);
- }
- if(rpm > 2699 and rpm < 2750)
- {
- AdjCW = (12);
- AdjACW = (57);
- }
- if(rpm > 2749 and rpm < 2825)
- {
- AdjCW = (13);
- AdjACW = (56);
- }
- if(rpm > 2824 and rpm < 2875)
- {
- AdjCW = (14);
- AdjACW = (55);
- }
- if(rpm > 2874)
- {
- AdjCW = (15);
- AdjACW = (54);
- }
+ rps = 1 / TimePerRev; //inverse to convert SPR to rps
+ rpm = rps * 60; //x 60 to convert rps to RPM
+
+ if(duty < 1) //Mapped Phase Advance with no load
+ {
+ if(rpm < 135.2 )
+ {
+ AdjCW = ( 0 );
+ AdjACW = ( 13 );
+ }
+ else if(rpm > 135.2 and rpm < 171.3 )
+ {
+ AdjCW = ( 1 );
+ AdjACW = ( 13 );
+ }
+ else if(rpm > 171.3 and rpm < 188.7 )
+ {
+ AdjCW = ( 1 );
+ AdjACW = ( 11 );
+ }
+ else if(rpm > 188.7 and rpm < 245.3 )
+ {
+ AdjCW = ( 2 );
+ AdjACW = ( 11 );
+ }
+ else if(rpm > 245.3 and rpm < 248.9 )
+ {
+ AdjCW = ( 2 );
+ AdjACW = ( 10 );
+ }
+ else if(rpm > 248.9 and rpm < 291.3 )
+ {
+ AdjCW = ( 3 );
+ AdjACW = ( 10 );
+ }
+ else if(rpm > 291.3 and rpm < 322.9 )
+ {
+ AdjCW = ( 3 );
+ AdjACW = ( 9 );
+ }
+ else if(rpm > 322.9 and rpm < 328.5 )
+ {
+ AdjCW = ( 3 );
+ AdjACW = ( 8 );
+ }
+ else if(rpm > 328.5 and rpm < 394.7 )
+ {
+ AdjCW = ( 4 );
+ AdjACW = ( 8 );
+ }
+ else if(rpm > 394.7 and rpm < 435.8 )
+ {
+ AdjCW = ( 4 );
+ AdjACW = ( 7 );
+ }
+ else if(rpm > 435.8 and rpm < 459.3 )
+ {
+ AdjCW = ( 5 );
+ AdjACW = ( 7 );
+ }
+ else if(rpm > 459.3 and rpm < 512.9 )
+ {
+ AdjCW = ( 5 );
+ AdjACW = ( 6 );
+ }
+ else if(rpm > 512.9 and rpm < 533.8 )
+ {
+ AdjCW = ( 6 );
+ AdjACW = ( 6 );
+ }
+ else if(rpm > 533.8 and rpm < 641.2 )
+ {
+ AdjCW = ( 6 );
+ AdjACW = ( 5 );
+ }
+ else if(rpm > 641.2 and rpm < 763.1 )
+ {
+ AdjCW = ( 7 );
+ AdjACW = ( 5 );
+ }
+ else if(rpm > 763.1 and rpm < 1002.3 )
+ {
+ AdjCW = ( 7 );
+ AdjACW = ( 4 );
+ }
+ else if(rpm > 1002.3 and rpm < 1318.6 )
+ {
+ AdjCW = ( 8 );
+ AdjACW = ( 4 );
+ }
+ else if(rpm > 1318.6 and rpm < 1338.1 )
+ {
+ AdjCW = ( 8 );
+ AdjACW = ( 3 );
+ }
+ else if(rpm > 1338.1 and rpm < 1463.6 )
+ {
+ AdjCW = ( 9 );
+ AdjACW = ( 3 );
+ }
+ else if(rpm > 1463.6 and rpm < 1493.7 )
+ {
+ AdjCW = ( 9 );
+ AdjACW = ( 2 );
+ }
+ else if(rpm > 1493.7 and rpm < 1719.6 )
+ {
+ AdjCW = ( 10 );
+ AdjACW = ( 2 );
+ }
+ else if(rpm > 1719.6 and rpm < 1780.5 )
+ {
+ AdjCW = ( 11 );
+ AdjACW = ( 2 );
+ }
+ else if(rpm > 1780.5 and rpm < 2030.5 )
+ {
+ AdjCW = ( 11 );
+ AdjACW = ( 1 );
+ }
+ else if(rpm > 2030.5 and rpm < 2073.1 )
+ {
+ AdjCW = ( 12 );
+ AdjACW = ( 1 );
+ }
+ else if(rpm > 2073.1 and rpm < 2255.2 )
+ {
+ AdjCW = ( 12 );
+ AdjACW = ( 0 );
+ }
+ else if(rpm > 2255.2 and rpm < 2463.3 )
+ {
+ AdjCW = ( 13 );
+ AdjACW = ( 0 );
+ }
+ else if(rpm > 2463.3 and rpm < 2479.4 )
+ {
+ AdjCW = ( 14 );
+ AdjACW = ( 0 );
+ }
+ else if(rpm > 2479.4 and rpm < 2591 )
+ {
+ AdjCW = ( 14 );
+ AdjACW = ( 63 );
+ }
+ else if(rpm > 2591 and rpm < 2612.9 )
+ {
+ AdjCW = ( 14 );
+ AdjACW = ( 62 );
+ }
+ else if(rpm > 2612.9 and rpm < 2742.4 )
+ {
+ AdjCW = ( 15 );
+ AdjACW = ( 62 );
+ }
+ else if(rpm > 2742.4 and rpm < 2762.3 )
+ {
+ AdjCW = ( 16 );
+ AdjACW = ( 62 );
+ }
+ else if(rpm > 2762.3 and rpm < 2894.6 )
+ {
+ AdjCW = ( 16 );
+ AdjACW = ( 61 );
+ }
+ else if(rpm > 2894.6 and rpm < 2926.7 )
+ {
+ AdjCW = ( 17 );
+ AdjACW = ( 61 );
+ }
+ else if(rpm > 2926.7 and rpm < 2982 )
+ {
+ AdjCW = ( 17 );
+ AdjACW = ( 59 );
+ }
+ else if(rpm > 2982 )
+ {
+ AdjCW = ( 17 );
+ AdjACW = ( 57 );
+ }
}
- if(duty > 0.99)
+
+ if(duty > 0.99) //Mapped Phase Advance with load
{
if(rpm < 48)
{
AdjCW = (52);
AdjACW = (19);
}
- if(rpm > 48 and rpm < 56)
+ else if(rpm > 48 and rpm < 56)
{
AdjCW = (53);
AdjACW = (19);
}
- if(rpm > 56 and rpm < 76)
+ else if(rpm > 56 and rpm < 76)
{
AdjCW = (55);
AdjACW = (19);
}
- if(rpm > 76 and rpm < 90)
+ else if(rpm > 76 and rpm < 90)
{
AdjCW = (58);
AdjACW = (19);
}
- if(rpm > 90 and rpm < 125)
+ else if(rpm > 90 and rpm < 125)
{
AdjCW = (58);
AdjACW = (14);
}
- if(rpm > 125 and rpm < 140)
+ else if(rpm > 125 and rpm < 140)
{
AdjCW = (62);
AdjACW = (14);
}
- if(rpm > 140 and rpm < 190)
+ else if(rpm > 140 and rpm < 190)
{
AdjCW = (62);
AdjACW = (9);
}
- if(rpm > 190 and rpm < 200)
+ else if(rpm > 190 and rpm < 200)
{
AdjCW = (3);
AdjACW = (9);
}
- if(rpm > 200 and rpm < 350)
+ else if(rpm > 200 and rpm < 350)
{
AdjCW = (3);
AdjACW = (9);
}
- if(rpm > 350 and rpm < 650)
+ else if(rpm > 350 and rpm < 650)
{
AdjCW = (3);
AdjACW = (5);
}
- if(rpm > 650)
+ else if(rpm > 650)
{
AdjCW = (6);
AdjACW = (5);
}
}
- //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*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)
- {
- j = 0;
- k = 0;
- l = 0;
- m = 0;
- n = 0;
- }
- if (duty > 0.79)
- {
- i = n-5;
- j = n-4;
- k = n-3;
- l = n-2;
- m = n-1;
- n = 6;
- }*/
- 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*gain); //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.11. 1 = max
{
duty = 1;
}
- if (duty <0.11) //3A min duty 0.96, 6.5A min duty 0.4
+ if (duty <0.11) //min duty 0.11
{
duty = 0.11;
}
- //pc.printf("%i, %.5f, %i, %i, %i \r", SetPoint, duty, AdjCW, AdjACW, rpm); //SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
}
-void ReadKType(void)
+void ReadKType(void) //Reads Temperature
{
int i = 0;
int Readout = 0;
cs1 = 0;
- SerialClock = 0; //set clock to 0
+ SerialClock = 0; //set clock to 0
wait_ms(0.01);
- SerialClock = 1; //clock once to set to the 13 bit temp data
+ SerialClock = 1; //clock once to set to the 13 bit temp data
wait_ms(0.01);
SerialClock = 0;
wait_ms(0.01);
- for(i = 13; i > -1; i = i-1) // now data is temp data where MSB is first and each count is worth 0.25 degrees
+ for(i = 13; i > -1; i = i-1) // now data is temp data where MSB is first and each count is worth 0.25 degrees
{
- if(DOut == 1) //check data, store results in readout
+ if(DOut == 1) //check data, store results in readout
{
Readout |= (1<<i);
}
@@ -1149,14 +984,12 @@
Readout |= (0<<i);
}
- SerialClock = 1; //clock to the next bit
+ SerialClock = 1; //clock to the next bit
wait_ms(0.01);
SerialClock = 0;
wait_ms(0.01);
}
- temp = Readout * 0.125; //get the real temp value which is a float
- //pc.printf("%.1f\r",temp);
- //pc.printf("%i, %.4f\n\r", rpm, duty);
+ temp = Readout * 0.125; //get the real temp value which is a float
Readout = 0;
cs1 = 1;
}
\ No newline at end of file