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-05-21
- Revision:
- 22:58558001804d
- Parent:
- 21:b831f68ce5ed
- Child:
- 24:ebaced951bbd
File content as of revision 22:58558001804d:
#include "mbed.h"
#include "QEI.h"
#include "TextLCD.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. When the motor is running the rpm is calculated every rotation
and then the PWM duty is adjusted until the desired rpm is achieved. The rpm is also used to change the phase advance and the PWM gain
so that it runs with the optimum setting at any speed and the changes in PWM duty give a smooth change in speed. The temperature
of the thrmocouple is measured every 0.6 seconds and will stop the motor if the temperature exceeds 80C.
*/
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);
void Switch (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
TextLCD lcd(p9, p10, p16, p17, p18, p19, TextLCD::LCD16x2);
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);
DigitalIn anticlockwise (p11);
DigitalOut SerialClock (p12); //ReadKType SPI
DigitalIn DOut (p13); //ReadKType SPI
DigitalOut cs1 (p14); //Chip Select SPI
DigitalIn clockwise (p15);
AnalogIn pot (p20); //Potentiometer for adjusting speed SetPoint
DigitalOut UnUsedPhase1 (p21);
DigitalOut UnUsedPhase2 (p22);
PwmOut Phase1 (p23);
PwmOut Phase2 (p24); //Pin set up - PWM to enable speed control
PwmOut Phase3 (p25);
PwmOut Phase4 (p26);
int SetPoint = 1500; //for adjusting the speed
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 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
int start = 28;
int adj = 25-start;
char c; //keyboard cotrol GetChar
float SetPointBuffer = 0; //Used in calculation to convert potentiometer value to SetPoint value. Used due to changing between int and float.
float rps = 0; //for calc of RPM
float rpm = 0; //for calc of RPM
float duty = 0.5; //PWM duty
float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
float gain = 0.0001; //Velocity loop
float p = 0.000014; //PWM period
float x=0.3; //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.000014 = 14 microseconds (500kHz). Good balance of low and high speed performance.
Phase2.period(p);
Phase3.period(p);
Phase4.period(p);
lcd.cls();
wait(2);
lcd.printf(" Switched\n");
lcd.locate(0,1);
lcd.printf("Reluctance Motor\n");
t.start(); //start timer for rpm calculation
SerialClock = 0; //reset serial clock for Themocouple
StepACW();
wait(0.1);
//duty=1; //Makes sure the motor starts from the same position each time
Initialisation(); //reset everything
while(wheel.getRevolutions()<4) //Index Calibration
{
StateA = (wheel.getPulses()+start)%s;
switch(StateA)
{
case 0:Ph1();pc.printf("1 %i\n\r", wheel.getPulses());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();pc.printf("2 %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;
case 33:Ph3();pc.printf("3 %i\n\r", wheel.getPulses());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();pc.printf("4 %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 used to increment the StateA count
{
StateA++; //increment StateA
wheel.ResetYay(); //reset Yay
if (StateA>(s-1)) //State A is only valid between 0 and 63 (64 states)
{
StateA=0; //continuous loop
}
}
}
while(1)
{
while((clockwise == 0) && (anticlockwise == 0)) //If no command to operate
{
duty = 0.11; //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
SetPointBuffer = (float) 3000 *pot; //cast pot value as a float
SetPoint = (int) SetPointBuffer; //Convert SetPointBuffer to an int and use as value for SetPoint
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
Switch();
lcd.cls();
wait(0.001);
lcd.printf("SRM STOP");
lcd.locate(0,1);
lcd.printf("%.0fRPM, %.0fdegC\n",rpm, temp);
wait(0.1);
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (clockwise==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
{
GetChar();
Switch(); //read keyboard strikes
StateB = (wheel.getPulses()+StateA+AdjCW-adj)%s; //calculation for stateB
SetPointBuffer = (float) 3000 *pot; //cast pot value as a float
SetPoint = (int) SetPointBuffer; //Convert SetPointBuffer to an int and use as value for SetPoint
switch(StateB)
{
case 0:Ph1();break;
case 1:Ph1();break;
case 2:Ph1();break;z
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.02*rpm)) //Reads temperature at a constant time interval
{
ReadKType(); //Read Temp
lcd.cls();
wait(0.0001);
lcd.printf("CW %.0f\n",rpm);
slowloop=0; //Reset slowloop
}
}
}
while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (clockwise==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
{
GetChar();
Switch();
StateB = (enc+wheel.getPulses()+StateA+AdjCW-adj)%s;
SetPointBuffer = (float) 3000 *pot;
SetPoint = (int) SetPointBuffer;
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();
lcd.cls();
wait(0.0001);
lcd.printf("CW %.0f\n",rpm);
slowloop=0;
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (anticlockwise==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
{
GetChar();
Switch();
StateB = (wheel.getPulses()+StateA+AdjACW-adj)%s;
SetPointBuffer = (float) 3000 *pot;
SetPoint = (int) SetPointBuffer;
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();
lcd.cls();
wait(0.0001);
lcd.printf("ACW %.0f\n",rpm);
slowloop=0;
}
}
}
while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (anticlockwise==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
{
GetChar();
Switch();
StateB = (enc+wheel.getPulses()+StateA+AdjACW-adj)%s;
SetPointBuffer = (float) 3000 *pot;
SetPoint = (int) SetPointBuffer;
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();
lcd.cls();
wait(0.0001);
lcd.printf("CW %.0f\n",rpm);
slowloop=0;
}
}
}
while(temp>(T-1)) //If Temp exceeds T the motor will not operate
{
Initialisation();
wait(0.001);
lcd.printf("SRM Over Temp");
lcd.locate(0,1);
lcd.printf("RPM=%.0f, %.0f C\n",rpm, temp);
wait(0.1);
pc.printf("Motor Over Temp\n\r");
while(1)
{
ReadKType(); //Read motor Temp
lcd.printf("SRM Over Temp");
lcd.locate(0,1);
lcd.printf("RPM=%.0f, %.0f C\n",rpm, temp);
wait(0.1);
pc.printf("%f\r",temp); //Display current Temp
wait(1);
if(temp<T-20) //Reset when Temp 20 degrees C below thermostat trip value
{
wait(0.001);
lcd.printf("SRM Back Online");
lcd.locate(0,1);
lcd.printf("RPM=%.0f, %.0f C\n",rpm, temp);
wait(0.1);
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
case 'm': pc.printf("%f \n\r", pot.read()); //Potentiometer value
}
}
}
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<299)
{
AdjCW = (57);
AdjACW = (12);
gain = (0.0001);
}
else if(rpm > 299 and rpm < 309)
{
AdjCW = (59);
AdjACW = (12);
gain = (0.0001);
}
else if(rpm > 309 and rpm < 319)
{
AdjCW = (60);
AdjACW = (12);
gain = (0.0001);
}
else if(rpm > 319 and rpm < 328)
{
AdjCW = (61);
AdjACW = (12);
gain = (0.0001);
}
else if(rpm > 328 and rpm < 337)
{
AdjCW = (62);
AdjACW = (12);
gain = (0.0001);
}
else if(rpm > 337 and rpm < 346)
{
AdjCW = (63);
AdjACW = (12);
gain = (0.0001);
}
else if(rpm > 346 and rpm < 351)
{
AdjCW = (0);
AdjACW = (12);
gain = (0.0001);
}
else if(rpm > 351 and rpm < 357)
{
AdjCW = (1);
AdjACW = (11);
gain = (0.0001);
}
else if(rpm > 357 and rpm < 362)
{
AdjCW = (3);
AdjACW = (10);
gain = (0.0001);
}
else if(rpm > 362 and rpm < 420)
{
AdjCW = (4);
AdjACW = (6);
gain = (0.0001);
}
else if(rpm > 420 and rpm < 548)
{
AdjCW = (5);
AdjACW = (4);
gain = (0.0001);
}
else if(rpm > 548 and rpm < 664)
{
AdjCW = (6);
AdjACW = (3);
gain = (0.0001);
}
else if(rpm > 664 and rpm < 710)
{
AdjCW = (7);
AdjACW = (3);
gain = (0.0001);
}
else if(rpm > 710 and rpm < 767)
{
AdjCW = (8);
AdjACW = (2);
gain = (0.0001);
}
else if(rpm > 767 and rpm < 960)
{
AdjCW = (9);
AdjACW = (1);
gain = (0.0001);
}
else if(rpm > 960 and rpm < 1051)
{
AdjCW = (10);
AdjACW = (0);
gain = (0.0001);
}
else if(rpm > 1051 and rpm < 1281)
{
AdjCW = (11);
AdjACW = (63);
gain = (0.00001);
}
else if(rpm > 1281 and rpm < 1427)
{
AdjCW = (12);
AdjACW = (62);
gain = (0.00001);
}
else if(rpm > 1427 and rpm < 1613)
{
AdjCW = (13);
AdjACW = (61);
gain = (0.00001);
}
else if(rpm > 1613 and rpm < 1661)
{
AdjCW = (14);
AdjACW = (60);
gain = (0.00001);
}
else if(rpm > 1661 and rpm < 1746)
{
AdjCW = (15);
AdjACW = (59);
gain = (0.00001);
}
else if(rpm > 1746 and rpm < 1999)
{
AdjCW = (16);
AdjACW = (58);
gain = (0.00001);
}
else if(rpm > 1999 and rpm < 2324)
{
AdjCW = (17);
AdjACW = (57);
gain = (0.00001);
}
else if(rpm > 2324 and rpm < 2621)
{
AdjCW = (18);
AdjACW = (56);
gain = (0.00001);
}
else if(rpm > 2621 and rpm < 2779)
{
AdjCW = (19);
AdjACW = (55);
gain = (0.00001);
}
else if(rpm > 2779 and rpm < 2882)
{
AdjCW = (20);
AdjACW = (54);
gain = (0.00001);
}
else if(rpm > 2882 and rpm < 2967)
{
AdjCW = (22);
AdjACW = (53);
gain = (0.000005);
}
else if(rpm > 2967)
{
AdjCW = (23);
AdjACW = (52);
gain = (0.000005);
}
}
/*if(rpm < 135)
{
AdjCW = (0);
AdjACW = (13);
gain = (0.0001); //proportional gain
}
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) //Speed control
{
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.053) //min duty 0.11
{
duty = 0.053;
}
}
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;
}
void Switch (void)
{
led1 = clockwise;
led2 = anticlockwise;
}