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:
- 24:ebaced951bbd
- Parent:
- 22:58558001804d
diff -r 9d85fc0217c5 -r ebaced951bbd main.cpp
--- a/main.cpp Tue May 21 14:38:56 2019 +0000
+++ b/main.cpp Wed May 19 10:43:30 2021 +0000
@@ -1,41 +1,15 @@
#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.
+/* Code written by Simon Truelove (18/05/2021) Stripped down version of SWM Demonstrator to check operation of TFM encoder RLS LM13ICPRGA00D10A and MR100F085A160M00
+using QEI.h. The code uses the pulses from the encoder to calculate RPM
*/
-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);
+QEI wheel(p5, p6, p8, 20000, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
Timer t; //timer used in RPM
@@ -44,724 +18,49 @@
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 enc = 80000; //20 000 x4 enc = 80 000 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();
+ pc.baud(460800); //Set baud rate
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;
+ 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
- }
+ led1==1;
+ pc.printf("yay= %i, whoop= %i, Pulses= %i, Revs= %i, RPM= %f\n\r", wheel.getYay(),wheel.getWhoop(),wheel.getPulses(),wheel.getRevolutions(), rpm);
+ }
+ if (pc.readable())
+ {
+ c = pc.getc();
+ if(c == 'z') //turn on led2 print some stuff
+ {
+ led2 = !led2;
+ pc.printf("yay= %i, whoop= %i, Pulses= %i, Revs= %i, RPM= %f\n\r", wheel.getYay(),wheel.getWhoop(),wheel.getPulses(),wheel.getRevolutions(), rpm);
}
}
-
- 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
@@ -771,512 +70,4 @@
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;
}
\ No newline at end of file