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.
main.cpp
- Committer:
- simontruelove
- Date:
- 2019-04-18
- Revision:
- 21:b831f68ce5ed
- Parent:
- 20:dca9f4c12fe3
- Child:
- 22:58558001804d
File content as of revision 21:b831f68ce5ed:
#include "mbed.h"
#include "QEI.h"
/* 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);
void Ph12 (void);
void Ph2(void);
void Ph23 (void);
void Ph3(void);
void Ph34 (void);
void Ph4(void);
void Ph41 (void);
void GetChar (void);
void RPM (void);
void VelocityLoop (void);
void ReadKType(void);
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
Timer t; //timer used in RPM
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 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 = 1500; //for adjusting the speed
int enc = 3200; //800 x4 enc = 3200 Pulses Per Rev
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 duty = 1;
float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
float gain = 0.0001; //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.
Phase2.period(p);
Phase3.period(p);
Phase4.period(p);
wait(0.1);
t.start();
SerialClock = 0;
StepACW();
Initialisation();
while(wheel.getRevolutions()<2) //Index Calibration
{
StateA = (wheel.getPulses()+25)%s;
switch(StateA)
{
case 0:Ph1();break;
case 1:Ph1();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;
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) //PulseCount_==1, yay_=1 used to increment the StateA count
{
StateA++;
wheel.ResetYay();
if (StateA>(s-1))
{
StateA=0;
}
}
}
while(1)
{
while((led1 == 0) && (led2 == 0)) //If no command to operate
{
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(); //read keyboard strikes
StateB = (wheel.getPulses()+StateA+AdjCW)%s; //calculation for stateB
switch(StateB)
{
case 0:Ph1();break;
case 1:Ph1();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:Ph0();break;
case 11:Ph1();break;
case 12:Ph1();break;
case 13:Ph0();break;
case 14:Ph1();break;
case 15:Ph0();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:Ph0();break;
case 27:Ph2();break;
case 28:Ph2();break;
case 29:Ph0();break;
case 30:Ph2();break;
case 31:Ph0();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:Ph0();break;
case 43:Ph3();break;
case 44:Ph3();break;
case 45:Ph0();break;
case 46:Ph3();break;
case 47:Ph0();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:Ph0();break;
case 59:Ph4();break;
case 60:Ph4();break;
case 61:Ph0();break;
case 62:Ph4();break;
case 63:Ph0();break;
default:break;
}
if(wheel.getWhoop()==1) //PulseCount2_==3200 (this is 1 rotation), whoop_=1;
{
RPM(); //calculate RPM
VelocityLoop(); //Adjust velocity
slowloop++; //increment slowloop
if(slowloop>(0.01*rpm)) //Reads temperature at a constant time interval
{
ReadKType(); //Read Temp
slowloop=0; //Reset slowloop
}
}
}
while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
{
GetChar();
StateB = (enc+wheel.getPulses()+StateA+AdjCW)%s;
switch(StateB)
{
case 0:Ph1();break;
case 1:Ph1();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:Ph0();break;
case 11:Ph1();break;
case 12:Ph1();break;
case 13:Ph0();break;
case 14:Ph1();break;
case 15:Ph0();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:Ph0();break;
case 27:Ph2();break;
case 28:Ph2();break;
case 29:Ph0();break;
case 30:Ph2();break;
case 31:Ph0();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:Ph0();break;
case 43:Ph3();break;
case 44:Ph3();break;
case 45:Ph0();break;
case 46:Ph3();break;
case 47:Ph0();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:Ph0();break;
case 59:Ph4();break;
case 60:Ph4();break;
case 61:Ph0();break;
case 62:Ph4();break;
case 63:Ph0();break;
default:break;
}
if(wheel.getWhoop()==1)
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>(0.01*rpm))
{
ReadKType();
slowloop=0;
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
{
GetChar();
StateB = (wheel.getPulses()+StateA+AdjACW)%s;
switch(StateB)
{
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:Ph0();break;
case 52:Ph4();break;
case 51:Ph4();break;
case 50:Ph0();break;
case 49:Ph4();break;
case 48:Ph0();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:Ph0();break;
case 36:Ph3();break;
case 35:Ph3();break;
case 34:Ph0();break;
case 33:Ph3();break;
case 32:Ph0();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:Ph0();break;
case 20:Ph2();break;
case 19:Ph2();break;
case 18:Ph0();break;
case 17:Ph2();break;
case 16:Ph0();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:Ph0();break;
case 4:Ph1();break;
case 3:Ph1();break;
case 2:Ph0();break;
case 1:Ph1();break;
case 0:Ph0();break;
default:break;
}
if(wheel.getWhoop()==1)
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>(0.01*rpm))
{
ReadKType();
slowloop=0;
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
{
GetChar();
StateB = (enc+wheel.getPulses()+StateA+AdjACW)%s;
switch(StateB)
{
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:Ph0();break;
case 52:Ph4();break;
case 51:Ph4();break;
case 50:Ph0();break;
case 49:Ph4();break;
case 48:Ph0();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:Ph0();break;
case 36:Ph3();break;
case 35:Ph3();break;
case 34:Ph0();break;
case 33:Ph3();break;
case 32:Ph0();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:Ph0();break;
case 20:Ph2();break;
case 19:Ph2();break;
case 18:Ph0();break;
case 17:Ph2();break;
case 16:Ph0();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:Ph0();break;
case 4:Ph1();break;
case 3:Ph1();break;
case 2:Ph0();break;
case 1:Ph1();break;
case 0:Ph0();break;
default:break;
}
if(wheel.getWhoop()==1)
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>(0.01*rpm))
{
ReadKType();
slowloop=0;
}
}
}
while(temp>(T-1)) //If Temp exceeds T the motor will not operate
{
Initialisation();
pc.printf("Motor Over Temp\n\r");
while(1)
{
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;
}
}
}
}
}
void StepACW(void) //Square wave switching
{
Ph4();
wait(x);
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) //Turn off all Phases
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(0);
Phase4.write(0);
}
void Ph1(void) //Turn on Phase 1
{
Phase1.write(duty);
Phase2.write(0);
Phase3.write(0);
Phase4.write(0);
}
void Ph12 (void) //Turn on Phase 1 and 2
{
Phase1.write(duty);
Phase2.write(duty);
Phase3.write(0);
Phase4.write(0);
}
void Ph2(void) //Turn on Phase 2
{
Phase1.write(0);
Phase2.write(duty);
Phase3.write(0);
Phase4.write(0);
}
void Ph23 (void) //Turn on Phase 2 and 3
{
Phase1.write(0);
Phase2.write(duty);
Phase3.write(duty);
Phase4.write(0);
}
void Ph3(void) //Turn on Phase 3
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(duty);
Phase4.write(0);
}
void Ph34 (void) //Turn on Phase 3 and 4
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(duty);
Phase4.write(duty);
}
void Ph4(void) //Turn on Phase 4
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(0);
Phase4.write(duty);
}
void Ph41 (void) //Turn on Phase 4 and 1
{
Phase1.write(duty);
Phase2.write(0);
Phase3.write(0);
Phase4.write(duty);
}
void Initialisation (void) //Turn everything off
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(0);
Phase4.write(0);
led1 = 0;
led2 = 0;
led3 = 0;
led4 = 0;
UnUsedPhase1=0;
UnUsedPhase2=0;
wheel.ResetYay();
wheel.QEI::reset();
}
void GetChar (void) //read keyboard strikes with terraterm
{ if (pc.readable())
{
c = pc.getc();
switch(c)
{
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
}
}
}
void RPM (void)
{
wheel.ResetWhoop(); //Whoop = 1 x per rev
TimePerClick = (t.read_us()); //read timer in microseconds
t.reset(); //reset timer
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
if(duty < 0.9) //Mapped Phase Advance with no load
{
if(rpm < 135)
{
AdjCW = (0);
AdjACW = (13);
gain = (0.0001);
}
else if(rpm > 135.2 and rpm < 171)
{
AdjCW = (1);
AdjACW = (13);
gain = (0.0001);
}
else if(rpm > 171 and rpm < 188)
{
AdjCW = (1);
AdjACW = (11);
gain = (0.0001);
}
else if(rpm > 188 and rpm < 245)
{
AdjCW = (2);
AdjACW = (11);
gain = (0.0001);
}
else if(rpm > 245 and rpm < 248)
{
AdjCW = (2);
AdjACW = (10);
gain = (0.0001);
}
else if(rpm > 248 and rpm < 291)
{
AdjCW = (3);
AdjACW = (10);
gain = (0.0001);
}
else if(rpm > 291 and rpm < 322)
{
AdjCW = (3);
AdjACW = (9);
gain = (0.0001);
}
else if(rpm > 322 and rpm < 328)
{
AdjCW = (3);
AdjACW = (8);
gain = (0.0001);
}
else if(rpm > 328 and rpm < 394)
{
AdjCW = (4);
AdjACW = (8);
gain = (0.0001);
}
else if(rpm > 394 and rpm < 435)
{
AdjCW = (4);
AdjACW = (7);
gain = (0.0001);
}
else if(rpm > 435 and rpm < 459)
{
AdjCW = (5);
AdjACW = (7);
gain = (0.0001);
}
else if(rpm > 459 and rpm < 512 )
{
AdjCW = (5);
AdjACW = (6);
gain = (0.0001);
}
else if(rpm > 512 and rpm < 533)
{
AdjCW = (6);
AdjACW = (6);
gain = (0.0001);
}
else if(rpm > 533 and rpm < 641)
{
AdjCW = (6);
AdjACW = (5);
gain = (0.0001);
}
else if(rpm > 641 and rpm < 763)
{
AdjCW = (7);
AdjACW = (5);
gain = (0.0001);
}
else if(rpm > 763 and rpm < 1002)
{
AdjCW = (7);
AdjACW = (4);
gain = (0.0001);
}
else if(rpm > 1002 and rpm < 1318)
{
AdjCW = (8);
AdjACW = (4);
gain = (0.00001);
}
else if(rpm > 1318 and rpm < 1338)
{
AdjCW = (8);
AdjACW = (3);
gain = (0.00001);
}
else if(rpm > 1338 and rpm < 1463)
{
AdjCW = (9);
AdjACW = (3);
gain = (0.00001);
}
else if(rpm > 1463 and rpm < 1493)
{
AdjCW = (9);
AdjACW = (2);
gain = (0.00001);
}
else if(rpm > 1493 and rpm < 1719)
{
AdjCW = (10);
AdjACW = (2);
gain = (0.00001);
}
else if(rpm > 1719 and rpm < 1780)
{
AdjCW = (11);
AdjACW = (2);
gain = (0.00001);
}
else if(rpm > 1780 and rpm < 2030)
{
AdjCW = (11);
AdjACW = (1);
gain = (0.00001);
}
else if(rpm > 2030 and rpm < 2073)
{
AdjCW = (12);
AdjACW = (1);
gain = (0.00001);
}
else if(rpm > 2073 and rpm < 2255)
{
AdjCW = (12);
AdjACW = (0);
gain = (0.00001);
}
else if(rpm > 2255 and rpm < 2463)
{
AdjCW = (13);
AdjACW = (0);
gain = (0.00001);
}
else if(rpm > 2463 and rpm < 2479)
{
AdjCW = (14);
AdjACW = (0);
gain = (0.00001);
}
else if(rpm > 2479 and rpm < 2591)
{
AdjCW = (14);
AdjACW = (63);
gain = (0.00001);
}
else if(rpm > 2591 and rpm < 2612)
{
AdjCW = (14);
AdjACW = (62);
gain = (0.00001);
}
else if(rpm > 2612 and rpm < 2742)
{
AdjCW = (15);
AdjACW = (62);
gain = (0.00001);
}
else if(rpm > 2742 and rpm < 2762)
{
AdjCW = (16);
AdjACW = (62);
gain = (0.00001);
}
else if(rpm > 2762 and rpm < 2894)
{
AdjCW = (16);
AdjACW = (61);
gain = (0.00001);
}
else if(rpm > 2894 and rpm < 2926)
{
AdjCW = (17);
AdjACW = (61);
gain = (0.000001);
}
else if(rpm > 2926 and rpm < 2982)
{
AdjCW = (17);
AdjACW = (59);
gain = (0.000001);
}
else if(rpm > 2982)
{
AdjCW = (17);
AdjACW = (57);
gain = (0.000001);
}
}
if(duty > 0.99) //Mapped Phase Advance with load
{
if(rpm < 48)
{
AdjCW = (52);
AdjACW = (19);
gain = (0.0001);
}
else if(rpm > 48 and rpm < 56)
{
AdjCW = (53);
AdjACW = (19);
gain = (0.0001);
}
else if(rpm > 56 and rpm < 76)
{
AdjCW = (55);
AdjACW = (19);
gain = (0.0001);
}
else if(rpm > 76 and rpm < 90)
{
AdjCW = (58);
AdjACW = (19);
gain = (0.0001);
}
else if(rpm > 90 and rpm < 125)
{
AdjCW = (58);
AdjACW = (14);
gain = (0.0001);
}
else if(rpm > 125 and rpm < 140)
{
AdjCW = (62);
AdjACW = (14);
gain = (0.0001);
}
else if(rpm > 140 and rpm < 190)
{
AdjCW = (62);
AdjACW = (9);
gain = (0.0001);
}
else if(rpm > 190 and rpm < 200)
{
AdjCW = (3);
AdjACW = (9);
gain = (0.0001);
}
else if(rpm > 200 and rpm < 350)
{
AdjCW = (3);
AdjACW = (9);
gain = (0.0001);
}
else if(rpm > 350 and rpm < 650)
{
AdjCW = (3);
AdjACW = (5);
gain = (0.0001);
}
else if(rpm > 650)
{
AdjCW = (6);
AdjACW = (5);
gain = (0.000001);
}
}
}
void VelocityLoop (void)
{
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) //min duty 0.11
{
duty = 0.11;
}
}
void ReadKType(void) //Reads Temperature
{
int i = 0;
int Readout = 0;
cs1 = 0;
SerialClock = 0; //set clock to 0
wait_ms(0.01);
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
{
if(DOut == 1) //check data, store results in readout
{
Readout |= (1<<i);
}
else
{
Readout |= (0<<i);
}
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
Readout = 0;
cs1 = 1;
}