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-02-06
- Revision:
- 16:e6c8df9960c6
- Parent:
- 15:12a4bbfa6de4
- Child:
- 17:19b2c598810a
File content as of revision 16:e6c8df9960c6:
#include "mbed.h"
#include "QEI.h"
void Initialisation (void); //These voids are written after the main. They must be listed here too (functional prototypes).
void 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
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);
DigitalOut UnUsedPhase1 (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
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 TimePerClick = 0; //for calc of RPM
int RPS = 0; //for calc of RPM
int rpm = 0; //for calc of RPM
int SetPoint = 1500; //for adjusting the speed
int enc = 3200;
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 * (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.00001;
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 temp = 0; //ReadKType
int main(void)
{
pc.baud(230400); //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(1)
{
}
*/
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 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;//;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>(s-1))
{
StateA=0;
}
}
}
while(1)
{
while((led1 == 0) && (led2 == 0)) //If no command to operate
{
//Aout = 0;
//duty = 0.7;
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());
}
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());
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.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;
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>(0.01*rpm))
{
ReadKType();
slowloop=0;
//pc.printf("%i, %.4f\n\r", rpm, duty);
}
}
}
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;
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.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;
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>(0.01*rpm))
{
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;
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.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;
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>(0.01*rpm))
{
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;
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.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;
{
RPM();
VelocityLoop();
slowloop++;
if(slowloop>(0.01*rpm))
{
ReadKType();
slowloop=0;
//pc.printf("%i, %.4f\n\r", rpm, duty);
}
}
}
while(temp>(T-1))
{
Initialisation();
pc.printf("Motor Over Temp\n\r");
while(1)
{
ReadKType();
pc.printf("%f\r",temp);
wait(1);
if(temp<T-20)
{
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)
{
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)
{
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)
{
Phase1.write(duty);
Phase2.write(duty);
Phase3.write(0);
Phase4.write(0);
//wait(y);
}
void Ph2(void)
{
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)
{
Phase1.write(0);
Phase2.write(duty);
Phase3.write(duty);
Phase4.write(0);
//wait(y);
}
void Ph3(void)
{
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)
{
Phase1.write(0);
Phase2.write(0);
Phase3.write(duty);
Phase4.write(duty);
//wait(y);
}
void Ph4(void)
{
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)
{
Phase1.write(duty);
Phase2.write(0);
Phase3.write(0);
Phase4.write(duty);
//wait(y);
}
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();
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 == '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 <50)
{
SetPoint = 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.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
{
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, %i, %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());
}
}
}
void RPM (void)
{
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(duty < 0.7)
{
if(rpm < 65)
{
AdjCW = (57);
AdjACW = (12);
}
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-n);
}
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);
}
}
if(duty > 0.69)
{
if(rpm < 175)
{
AdjCW = (60);
AdjACW = (11);
}
if(rpm > 200 and rpm < 275)
{
AdjCW = (5);
AdjACW = (11);
}if(rpm > 300 and rpm < 1000)
{
AdjCW = (5);
AdjACW = (5);
}
}
/*if(rpm < 180)
{
AdjCW = (57+n);
AdjACW = (12-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 179 and rpm <195)
{
AdjCW = (58+n);
AdjACW = (11-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 194 and rpm <220)
{
AdjCW = (59+n);
AdjACW = (10-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 219 and rpm <240)
{
AdjCW = (60+n);
AdjACW = (9-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 239 and rpm <295)
{
AdjCW = (61+n);
AdjACW = (8-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 294 and rpm <325)
{
AdjCW = (62+n);
AdjACW = (7-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 324 and rpm <355)
{
AdjCW = (63+n);
AdjACW = (6-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 354 and rpm <415)
{
AdjCW = (0+n);
AdjACW = (5-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 414 and rpm <595)
{
AdjCW = (1+n);
AdjACW = (4-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 594 and rpm <655)
{
AdjCW = (2+n);
AdjACW = (3-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 654 and rpm <765)
{
AdjCW = (3+n);
AdjACW = (2-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 764 and rpm <1010)
{
AdjCW = (4+n);
AdjACW = (1-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 1009 and rpm <1150)
{
AdjCW = (5+n);
AdjACW = (0-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 1149 and rpm <1225)
{
AdjCW = (6+n);
AdjACW = (63-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 1224 and rpm <1310)
{
AdjCW = (7+n);
AdjACW = (62-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 1309 and rpm <1670)
{
AdjCW = (8+n);
AdjACW = (61-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 1669 and rpm <2035)
{
AdjCW = (9+n);
AdjACW = (60-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 2034 and rpm <2175)
{
AdjCW = (10+n);
AdjACW = (59-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 2174 and rpm <2275)
{
AdjCW = (11+n);
AdjACW = (58-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 2274 and rpm <2515)
{
AdjCW = (12+n);
AdjACW = (57-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 2514 and rpm <2575)
{
AdjCW = (13+n);
AdjACW = (56-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 2574 and rpm <2755)
{
AdjCW = (14+n);
AdjACW = (55-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 2754 and rpm <2835)
{
AdjCW = (15+n);
AdjACW = (54-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 2834 and rpm <2860)
{
AdjCW = (16+n);
AdjACW = (53-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}
if(rpm > 2861)
{
AdjCW = (17+n);
AdjACW = (52-n);
if (AdjCW >(s-1))
{
AdjCW = 0;
}
if (AdjCW <0)
{
AdjCW = (s-1);
}
if (AdjACW <0)
{
AdjACW = (s-1);
}
if (AdjACW >(s-1))
{
AdjACW = 0;
}
}*/
//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
{
duty = 1;
}
if (duty <0.08) //3A min duty 0.96, 6.5A min duty 0.4
{
duty = 0.08;
}
//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)
{
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
//pc.printf("%.1f\r",temp);
//pc.printf("%i, %.4f\n\r", rpm, duty);
Readout = 0;
cs1 = 1;
}