Rotork Research Team / Mbed 2 deprecated 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.

Dependencies:   mbed QEI

main.cpp@22:58558001804d, 2019-05-21 (annotated)

Committer:
simontruelove
Date:
Tue May 21 11:56:25 2019 +0000
Revision:
22:58558001804d
Parent:
21:b831f68ce5ed
Child:
24:ebaced951bbd
Demo version using 18V 5A Power supply. No loaded phase advance

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simontruelove 0:634dd505dace 1 #include "mbed.h"
simontruelove 3:4249dbdf7ed3 2 #include "QEI.h"
simontruelove 22:58558001804d 3 #include "TextLCD.h"
simontruelove 3:4249dbdf7ed3 4
simontruelove 20:dca9f4c12fe3 5 /* Code written by Simon Truelove (completed 5/4/2019) to control Switched Reluctance Motor (SRM) modified from a
simontruelove 20:dca9f4c12fe3 6 QSH6018-65-28-210 stepper motor. The position of the motor is monitored using a rotary encoder 102-1307-ND using QEI.h.
simontruelove 20:dca9f4c12fe3 7 The code uses the pulses from the encoder to increment the values StateA and StateB to control switching of the phases needed
simontruelove 20:dca9f4c12fe3 8 to generate rotation via a 64 case state machine. Upon start up the motor will step anti clockwise by switching each phase
simontruelove 20:dca9f4c12fe3 9 on in sequence then it will complete 2 revolutions to find the encoder index and set StateA which is used for calculating
simontruelove 20:dca9f4c12fe3 10 StateB. StateB controls 4 identical state machines, however state B is calculated slightly differently within each one
simontruelove 20:dca9f4c12fe3 11 depending on the direction of rotation required and the direction the motor had turned previously. LED's are switched on and
simontruelove 22:58558001804d 12 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
simontruelove 22:58558001804d 13 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
simontruelove 22:58558001804d 14 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
simontruelove 22:58558001804d 15 of the thrmocouple is measured every 0.6 seconds and will stop the motor if the temperature exceeds 80C.
simontruelove 20:dca9f4c12fe3 16 */
simontruelove 20:dca9f4c12fe3 17
simontruelove 20:dca9f4c12fe3 18 void Initialisation (void); //These function prototypes are written after the main. They must be listed here too
simontruelove 16:e6c8df9960c6 19 void StepACW(void);
simontruelove 13:da9d3fbbe407 20 void Ph0(void);
simontruelove 1:0191658b6ff4 21 void Ph1(void);
simontruelove 3:4249dbdf7ed3 22 void Ph12 (void);
simontruelove 1:0191658b6ff4 23 void Ph2(void);
simontruelove 3:4249dbdf7ed3 24 void Ph23 (void);
simontruelove 1:0191658b6ff4 25 void Ph3(void);
simontruelove 3:4249dbdf7ed3 26 void Ph34 (void);
simontruelove 1:0191658b6ff4 27 void Ph4(void);
simontruelove 3:4249dbdf7ed3 28 void Ph41 (void);
simontruelove 4:3aedc9246ae4 29 void GetChar (void);
simontruelove 5:4e5c644d5cc3 30 void RPM (void);
simontruelove 7:b8de1529c7fc 31 void VelocityLoop (void);
simontruelove 11:74eeb8871fe6 32 void ReadKType(void);
simontruelove 22:58558001804d 33 void Switch (void);
simontruelove 1:0191658b6ff4 34
simontruelove 10:808cb9052f14 35 Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
simontruelove 1:0191658b6ff4 36
simontruelove 10:808cb9052f14 37 QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
simontruelove 22:58558001804d 38 TextLCD lcd(p9, p10, p16, p17, p18, p19, TextLCD::LCD16x2);
simontruelove 3:4249dbdf7ed3 39
simontruelove 10:808cb9052f14 40 Timer t; //timer used in RPM
simontruelove 3:4249dbdf7ed3 41
simontruelove 20:dca9f4c12fe3 42 DigitalOut led1 (LED1); //LEDs used to as very basic memmory for controlling the state machines
simontruelove 20:dca9f4c12fe3 43 DigitalOut led2 (LED2);
simontruelove 20:dca9f4c12fe3 44 DigitalOut led3 (LED3);
simontruelove 20:dca9f4c12fe3 45 DigitalOut led4 (LED4);
simontruelove 22:58558001804d 46 DigitalIn anticlockwise (p11);
simontruelove 20:dca9f4c12fe3 47 DigitalOut SerialClock (p12); //ReadKType SPI
simontruelove 20:dca9f4c12fe3 48 DigitalIn DOut (p13); //ReadKType SPI
simontruelove 20:dca9f4c12fe3 49 DigitalOut cs1 (p14); //Chip Select SPI
simontruelove 22:58558001804d 50 DigitalIn clockwise (p15);
simontruelove 22:58558001804d 51
simontruelove 22:58558001804d 52 AnalogIn pot (p20); //Potentiometer for adjusting speed SetPoint
simontruelove 20:dca9f4c12fe3 53 DigitalOut UnUsedPhase1 (p21);
simontruelove 22:58558001804d 54 DigitalOut UnUsedPhase2 (p22);
simontruelove 22:58558001804d 55 PwmOut Phase1 (p23);
simontruelove 22:58558001804d 56 PwmOut Phase2 (p24); //Pin set up - PWM to enable speed control
simontruelove 22:58558001804d 57 PwmOut Phase3 (p25);
simontruelove 22:58558001804d 58 PwmOut Phase4 (p26);
simontruelove 10:808cb9052f14 59
simontruelove 22:58558001804d 60 int SetPoint = 1500; //for adjusting the speed
simontruelove 10:808cb9052f14 61 int StateA = 0; //State for first 2 revolutions (calibration of the index)
simontruelove 10:808cb9052f14 62 int StateB = 0; //All state machines after calibration use this state
simontruelove 20:dca9f4c12fe3 63 int AdjCW = 57; //CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 20:dca9f4c12fe3 64 int AdjACW = 12; //ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 20:dca9f4c12fe3 65 int CW = 57; //Value used to start motor from stationary
simontruelove 20:dca9f4c12fe3 66 int ACW = 12; //Value used to start motor from stationary
simontruelove 10:808cb9052f14 67 int TimePerClick = 0; //for calc of RPM
simontruelove 17:19b2c598810a 68 int enc = 3200; //800 x4 enc = 3200 Pulses Per Rev
simontruelove 20:dca9f4c12fe3 69 int s = enc/50; //s=number of cases in state machine (64)
simontruelove 20:dca9f4c12fe3 70 int T = 80; //Thermostat protection limit degrees centigrade
simontruelove 20:dca9f4c12fe3 71 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
simontruelove 20:dca9f4c12fe3 72 int slowloop = 0; //Counter used to read temp at a slower rate
simontruelove 22:58558001804d 73 int start = 28;
simontruelove 22:58558001804d 74 int adj = 25-start;
simontruelove 11:74eeb8871fe6 75
simontruelove 10:808cb9052f14 76 char c; //keyboard cotrol GetChar
simontruelove 4:3aedc9246ae4 77
simontruelove 22:58558001804d 78 float SetPointBuffer = 0; //Used in calculation to convert potentiometer value to SetPoint value. Used due to changing between int and float.
simontruelove 20:dca9f4c12fe3 79 float rps = 0; //for calc of RPM
simontruelove 20:dca9f4c12fe3 80 float rpm = 0; //for calc of RPM
simontruelove 22:58558001804d 81 float duty = 0.5; //PWM duty
simontruelove 10:808cb9052f14 82 float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
simontruelove 22:58558001804d 83 float gain = 0.0001; //Velocity loop
simontruelove 20:dca9f4c12fe3 84 float p = 0.000014; //PWM period
simontruelove 22:58558001804d 85 float x=0.3; //Used in StepACW x=time of square wave when 1 phase energised,
simontruelove 20:dca9f4c12fe3 86 float TimePerRev = 0; //for calc of RPM
simontruelove 20:dca9f4c12fe3 87 float y=0.04; //Used in Step ACWy=time of square wave when 2 phases energised
simontruelove 10:808cb9052f14 88 float temp = 0; //ReadKType
simontruelove 1:0191658b6ff4 89
simontruelove 1:0191658b6ff4 90 int main(void)
simontruelove 3:4249dbdf7ed3 91 {
simontruelove 17:19b2c598810a 92 pc.baud(921600); //Set fastest baud rate
simontruelove 22:58558001804d 93 Phase1.period(p); //period of 0.000014 = 14 microseconds (500kHz). Good balance of low and high speed performance.
simontruelove 16:e6c8df9960c6 94 Phase2.period(p);
simontruelove 16:e6c8df9960c6 95 Phase3.period(p);
simontruelove 16:e6c8df9960c6 96 Phase4.period(p);
simontruelove 22:58558001804d 97 lcd.cls();
simontruelove 22:58558001804d 98 wait(2);
simontruelove 22:58558001804d 99 lcd.printf(" Switched\n");
simontruelove 22:58558001804d 100 lcd.locate(0,1);
simontruelove 22:58558001804d 101 lcd.printf("Reluctance Motor\n");
simontruelove 22:58558001804d 102 t.start(); //start timer for rpm calculation
simontruelove 22:58558001804d 103 SerialClock = 0; //reset serial clock for Themocouple
simontruelove 22:58558001804d 104 StepACW();
simontruelove 14:1eb49362a607 105 wait(0.1);
simontruelove 22:58558001804d 106 //duty=1; //Makes sure the motor starts from the same position each time
simontruelove 22:58558001804d 107 Initialisation(); //reset everything
simontruelove 22:58558001804d 108
simontruelove 22:58558001804d 109 while(wheel.getRevolutions()<4) //Index Calibration
simontruelove 20:dca9f4c12fe3 110 {
simontruelove 22:58558001804d 111 StateA = (wheel.getPulses()+start)%s;
simontruelove 5:4e5c644d5cc3 112 switch(StateA)
simontruelove 3:4249dbdf7ed3 113 {
simontruelove 22:58558001804d 114 case 0:Ph1();pc.printf("1 %i\n\r", wheel.getPulses());break;
simontruelove 6:f7028034aabb 115 case 1:Ph1();break;
simontruelove 14:1eb49362a607 116 case 2:Ph1();break;
simontruelove 14:1eb49362a607 117 case 3:Ph1();break;
simontruelove 14:1eb49362a607 118 case 4:Ph1();break;
simontruelove 14:1eb49362a607 119 case 5:Ph1();break;
simontruelove 14:1eb49362a607 120 case 6:Ph1();break;
simontruelove 14:1eb49362a607 121 case 7:Ph1();break;
simontruelove 14:1eb49362a607 122 case 8:Ph1();break;
simontruelove 14:1eb49362a607 123 case 9:Ph1();break;
simontruelove 14:1eb49362a607 124 case 10:Ph1();break;
simontruelove 14:1eb49362a607 125 case 11:Ph1();break;
simontruelove 14:1eb49362a607 126 case 12:Ph1();break;
simontruelove 14:1eb49362a607 127 case 13:Ph1();break;
simontruelove 14:1eb49362a607 128 case 14:Ph1();break;
simontruelove 14:1eb49362a607 129 case 15:Ph1();break;
simontruelove 22:58558001804d 130 case 16:Ph2();pc.printf("2 %i\n\r", wheel.getPulses());break;
simontruelove 14:1eb49362a607 131 case 17:Ph2();break;
simontruelove 14:1eb49362a607 132 case 18:Ph2();break;
simontruelove 14:1eb49362a607 133 case 19:Ph2();break;
simontruelove 14:1eb49362a607 134 case 20:Ph2();break;
simontruelove 14:1eb49362a607 135 case 21:Ph2();break;
simontruelove 14:1eb49362a607 136 case 22:Ph2();break;
simontruelove 14:1eb49362a607 137 case 23:Ph2();break;
simontruelove 14:1eb49362a607 138 case 24:Ph2();break;
simontruelove 14:1eb49362a607 139 case 25:Ph2();break;
simontruelove 14:1eb49362a607 140 case 26:Ph2();break;
simontruelove 14:1eb49362a607 141 case 27:Ph2();break;
simontruelove 14:1eb49362a607 142 case 28:Ph2();break;
simontruelove 14:1eb49362a607 143 case 29:Ph2();break;
simontruelove 14:1eb49362a607 144 case 30:Ph2();break;
simontruelove 14:1eb49362a607 145 case 31:Ph2();break;
simontruelove 20:dca9f4c12fe3 146 case 32:Ph3();break;
simontruelove 22:58558001804d 147 case 33:Ph3();pc.printf("3 %i\n\r", wheel.getPulses());break;
simontruelove 14:1eb49362a607 148 case 34:Ph3();break;
simontruelove 14:1eb49362a607 149 case 35:Ph3();break;
simontruelove 14:1eb49362a607 150 case 36:Ph3();break;
simontruelove 14:1eb49362a607 151 case 37:Ph3();break;
simontruelove 14:1eb49362a607 152 case 38:Ph3();break;
simontruelove 14:1eb49362a607 153 case 39:Ph3();break;
simontruelove 14:1eb49362a607 154 case 40:Ph3();break;
simontruelove 14:1eb49362a607 155 case 41:Ph3();break;
simontruelove 14:1eb49362a607 156 case 42:Ph3();break;
simontruelove 14:1eb49362a607 157 case 43:Ph3();break;
simontruelove 14:1eb49362a607 158 case 44:Ph3();break;
simontruelove 14:1eb49362a607 159 case 45:Ph3();break;
simontruelove 14:1eb49362a607 160 case 46:Ph3();break;
simontruelove 14:1eb49362a607 161 case 47:Ph3();break;
simontruelove 22:58558001804d 162 case 48:Ph4();pc.printf("4 %i\n\r", wheel.getPulses());break;
simontruelove 14:1eb49362a607 163 case 49:Ph4();break;
simontruelove 14:1eb49362a607 164 case 50:Ph4();break;
simontruelove 14:1eb49362a607 165 case 51:Ph4();break;
simontruelove 14:1eb49362a607 166 case 52:Ph4();break;
simontruelove 14:1eb49362a607 167 case 53:Ph4();break;
simontruelove 14:1eb49362a607 168 case 54:Ph4();break;
simontruelove 14:1eb49362a607 169 case 55:Ph4();break;
simontruelove 14:1eb49362a607 170 case 56:Ph4();break;
simontruelove 14:1eb49362a607 171 case 57:Ph4();break;
simontruelove 14:1eb49362a607 172 case 58:Ph4();break;
simontruelove 14:1eb49362a607 173 case 59:Ph4();break;
simontruelove 14:1eb49362a607 174 case 60:Ph4();break;
simontruelove 14:1eb49362a607 175 case 61:Ph4();break;
simontruelove 14:1eb49362a607 176 case 62:Ph4();break;
simontruelove 14:1eb49362a607 177 case 63:Ph4();break;
simontruelove 5:4e5c644d5cc3 178 default:break;
simontruelove 3:4249dbdf7ed3 179 }
simontruelove 20:dca9f4c12fe3 180 if(wheel.getYay()==1) //PulseCount_==1, yay_=1 used to increment the StateA count
simontruelove 3:4249dbdf7ed3 181 {
simontruelove 22:58558001804d 182 StateA++; //increment StateA
simontruelove 22:58558001804d 183 wheel.ResetYay(); //reset Yay
simontruelove 22:58558001804d 184 if (StateA>(s-1)) //State A is only valid between 0 and 63 (64 states)
simontruelove 3:4249dbdf7ed3 185 {
simontruelove 22:58558001804d 186 StateA=0; //continuous loop
simontruelove 3:4249dbdf7ed3 187 }
simontruelove 3:4249dbdf7ed3 188 }
simontruelove 14:1eb49362a607 189 }
simontruelove 3:4249dbdf7ed3 190
simontruelove 5:4e5c644d5cc3 191 while(1)
simontruelove 5:4e5c644d5cc3 192 {
simontruelove 22:58558001804d 193 while((clockwise == 0) && (anticlockwise == 0)) //If no command to operate
simontruelove 4:3aedc9246ae4 194 {
simontruelove 22:58558001804d 195 duty = 0.11; //Duty reduced to low value to ensure ramp up of speed
simontruelove 20:dca9f4c12fe3 196 rpm = 0; //RPM function not being triggered due to no rotation. RPM set to 0
simontruelove 22:58558001804d 197 SetPointBuffer = (float) 3000 *pot; //cast pot value as a float
simontruelove 22:58558001804d 198 SetPoint = (int) SetPointBuffer; //Convert SetPointBuffer to an int and use as value for SetPoint
simontruelove 20:dca9f4c12fe3 199 AdjCW = CW; //Reset to correct value for start up
simontruelove 20:dca9f4c12fe3 200 AdjACW = ACW; //Reset to correct value for start up
simontruelove 20:dca9f4c12fe3 201 Ph0(); //turn off all phases
simontruelove 20:dca9f4c12fe3 202 GetChar(); //read keyboard strikes
simontruelove 20:dca9f4c12fe3 203 ReadKType(); //Temperature measurement
simontruelove 20:dca9f4c12fe3 204 StateB = (wheel.getPulses()+StateA)%s; //calculation for stateB
simontruelove 22:58558001804d 205 Switch();
simontruelove 22:58558001804d 206 lcd.cls();
simontruelove 22:58558001804d 207 wait(0.001);
simontruelove 22:58558001804d 208 lcd.printf("SRM STOP");
simontruelove 22:58558001804d 209 lcd.locate(0,1);
simontruelove 22:58558001804d 210 lcd.printf("%.0fRPM, %.0fdegC\n",rpm, temp);
simontruelove 22:58558001804d 211 wait(0.1);
simontruelove 4:3aedc9246ae4 212 }
simontruelove 22:58558001804d 213 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (clockwise==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
simontruelove 3:4249dbdf7ed3 214 {
simontruelove 22:58558001804d 215 GetChar();
simontruelove 22:58558001804d 216 Switch(); //read keyboard strikes
simontruelove 22:58558001804d 217 StateB = (wheel.getPulses()+StateA+AdjCW-adj)%s; //calculation for stateB
simontruelove 22:58558001804d 218 SetPointBuffer = (float) 3000 *pot; //cast pot value as a float
simontruelove 22:58558001804d 219 SetPoint = (int) SetPointBuffer; //Convert SetPointBuffer to an int and use as value for SetPoint
simontruelove 4:3aedc9246ae4 220
simontruelove 4:3aedc9246ae4 221 switch(StateB)
simontruelove 4:3aedc9246ae4 222 {
simontruelove 6:f7028034aabb 223 case 0:Ph1();break;
simontruelove 6:f7028034aabb 224 case 1:Ph1();break;
simontruelove 22:58558001804d 225 case 2:Ph1();break;z
simontruelove 13:da9d3fbbe407 226 case 3:Ph1();break;
simontruelove 14:1eb49362a607 227 case 4:Ph1();break;
simontruelove 14:1eb49362a607 228 case 5:Ph1();break;
simontruelove 14:1eb49362a607 229 case 6:Ph1();break;
simontruelove 14:1eb49362a607 230 case 7:Ph1();break;
simontruelove 14:1eb49362a607 231 case 8:Ph1();break;
simontruelove 14:1eb49362a607 232 case 9:Ph1();break;
simontruelove 16:e6c8df9960c6 233 case 10:Ph0();break;
simontruelove 14:1eb49362a607 234 case 11:Ph1();break;
simontruelove 14:1eb49362a607 235 case 12:Ph1();break;
simontruelove 16:e6c8df9960c6 236 case 13:Ph0();break;
simontruelove 14:1eb49362a607 237 case 14:Ph1();break;
simontruelove 16:e6c8df9960c6 238 case 15:Ph0();break;
simontruelove 14:1eb49362a607 239 case 16:Ph2();break;
simontruelove 14:1eb49362a607 240 case 17:Ph2();break;
simontruelove 14:1eb49362a607 241 case 18:Ph2();break;
simontruelove 14:1eb49362a607 242 case 19:Ph2();break;
simontruelove 14:1eb49362a607 243 case 20:Ph2();break;
simontruelove 14:1eb49362a607 244 case 21:Ph2();break;
simontruelove 14:1eb49362a607 245 case 22:Ph2();break;
simontruelove 14:1eb49362a607 246 case 23:Ph2();break;
simontruelove 14:1eb49362a607 247 case 24:Ph2();break;
simontruelove 14:1eb49362a607 248 case 25:Ph2();break;
simontruelove 16:e6c8df9960c6 249 case 26:Ph0();break;
simontruelove 14:1eb49362a607 250 case 27:Ph2();break;
simontruelove 14:1eb49362a607 251 case 28:Ph2();break;
simontruelove 16:e6c8df9960c6 252 case 29:Ph0();break;
simontruelove 14:1eb49362a607 253 case 30:Ph2();break;
simontruelove 16:e6c8df9960c6 254 case 31:Ph0();break;
simontruelove 14:1eb49362a607 255 case 32:Ph3();break;
simontruelove 14:1eb49362a607 256 case 33:Ph3();break;
simontruelove 14:1eb49362a607 257 case 34:Ph3();break;
simontruelove 14:1eb49362a607 258 case 35:Ph3();break;
simontruelove 14:1eb49362a607 259 case 36:Ph3();break;
simontruelove 14:1eb49362a607 260 case 37:Ph3();break;
simontruelove 14:1eb49362a607 261 case 38:Ph3();break;
simontruelove 14:1eb49362a607 262 case 39:Ph3();break;
simontruelove 14:1eb49362a607 263 case 40:Ph3();break;
simontruelove 14:1eb49362a607 264 case 41:Ph3();break;
simontruelove 16:e6c8df9960c6 265 case 42:Ph0();break;
simontruelove 14:1eb49362a607 266 case 43:Ph3();break;
simontruelove 14:1eb49362a607 267 case 44:Ph3();break;
simontruelove 16:e6c8df9960c6 268 case 45:Ph0();break;
simontruelove 14:1eb49362a607 269 case 46:Ph3();break;
simontruelove 16:e6c8df9960c6 270 case 47:Ph0();break;
simontruelove 14:1eb49362a607 271 case 48:Ph4();break;
simontruelove 14:1eb49362a607 272 case 49:Ph4();break;
simontruelove 14:1eb49362a607 273 case 50:Ph4();break;
simontruelove 14:1eb49362a607 274 case 51:Ph4();break;
simontruelove 14:1eb49362a607 275 case 52:Ph4();break;
simontruelove 14:1eb49362a607 276 case 53:Ph4();break;
simontruelove 14:1eb49362a607 277 case 54:Ph4();break;
simontruelove 14:1eb49362a607 278 case 55:Ph4();break;
simontruelove 14:1eb49362a607 279 case 56:Ph4();break;
simontruelove 14:1eb49362a607 280 case 57:Ph4();break;
simontruelove 16:e6c8df9960c6 281 case 58:Ph0();break;
simontruelove 14:1eb49362a607 282 case 59:Ph4();break;
simontruelove 14:1eb49362a607 283 case 60:Ph4();break;
simontruelove 16:e6c8df9960c6 284 case 61:Ph0();break;
simontruelove 14:1eb49362a607 285 case 62:Ph4();break;
simontruelove 16:e6c8df9960c6 286 case 63:Ph0();break;
simontruelove 16:e6c8df9960c6 287 default:break;
simontruelove 4:3aedc9246ae4 288 }
simontruelove 20:dca9f4c12fe3 289
simontruelove 20:dca9f4c12fe3 290 if(wheel.getWhoop()==1) //PulseCount2_==3200 (this is 1 rotation), whoop_=1;
simontruelove 4:3aedc9246ae4 291 {
simontruelove 20:dca9f4c12fe3 292 RPM(); //calculate RPM
simontruelove 20:dca9f4c12fe3 293 VelocityLoop(); //Adjust velocity
simontruelove 20:dca9f4c12fe3 294 slowloop++; //increment slowloop
simontruelove 22:58558001804d 295 if(slowloop>(0.02*rpm)) //Reads temperature at a constant time interval
simontruelove 14:1eb49362a607 296 {
simontruelove 20:dca9f4c12fe3 297 ReadKType(); //Read Temp
simontruelove 22:58558001804d 298 lcd.cls();
simontruelove 22:58558001804d 299 wait(0.0001);
simontruelove 22:58558001804d 300 lcd.printf("CW %.0f\n",rpm);
simontruelove 22:58558001804d 301 slowloop=0; //Reset slowloop
simontruelove 14:1eb49362a607 302 }
simontruelove 11:74eeb8871fe6 303 }
simontruelove 4:3aedc9246ae4 304 }
simontruelove 4:3aedc9246ae4 305
simontruelove 22:58558001804d 306 while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (clockwise==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
simontruelove 4:3aedc9246ae4 307 {
simontruelove 4:3aedc9246ae4 308 GetChar();
simontruelove 22:58558001804d 309 Switch();
simontruelove 22:58558001804d 310 StateB = (enc+wheel.getPulses()+StateA+AdjCW-adj)%s;
simontruelove 22:58558001804d 311 SetPointBuffer = (float) 3000 *pot;
simontruelove 22:58558001804d 312 SetPoint = (int) SetPointBuffer;
simontruelove 20:dca9f4c12fe3 313
simontruelove 3:4249dbdf7ed3 314 switch(StateB)
simontruelove 3:4249dbdf7ed3 315 {
simontruelove 6:f7028034aabb 316 case 0:Ph1();break;
simontruelove 6:f7028034aabb 317 case 1:Ph1();break;
simontruelove 13:da9d3fbbe407 318 case 2:Ph1();break;
simontruelove 13:da9d3fbbe407 319 case 3:Ph1();break;
simontruelove 14:1eb49362a607 320 case 4:Ph1();break;
simontruelove 14:1eb49362a607 321 case 5:Ph1();break;
simontruelove 14:1eb49362a607 322 case 6:Ph1();break;
simontruelove 14:1eb49362a607 323 case 7:Ph1();break;
simontruelove 14:1eb49362a607 324 case 8:Ph1();break;
simontruelove 14:1eb49362a607 325 case 9:Ph1();break;
simontruelove 16:e6c8df9960c6 326 case 10:Ph0();break;
simontruelove 14:1eb49362a607 327 case 11:Ph1();break;
simontruelove 14:1eb49362a607 328 case 12:Ph1();break;
simontruelove 16:e6c8df9960c6 329 case 13:Ph0();break;
simontruelove 14:1eb49362a607 330 case 14:Ph1();break;
simontruelove 16:e6c8df9960c6 331 case 15:Ph0();break;
simontruelove 14:1eb49362a607 332 case 16:Ph2();break;
simontruelove 14:1eb49362a607 333 case 17:Ph2();break;
simontruelove 14:1eb49362a607 334 case 18:Ph2();break;
simontruelove 14:1eb49362a607 335 case 19:Ph2();break;
simontruelove 14:1eb49362a607 336 case 20:Ph2();break;
simontruelove 14:1eb49362a607 337 case 21:Ph2();break;
simontruelove 14:1eb49362a607 338 case 22:Ph2();break;
simontruelove 14:1eb49362a607 339 case 23:Ph2();break;
simontruelove 14:1eb49362a607 340 case 24:Ph2();break;
simontruelove 14:1eb49362a607 341 case 25:Ph2();break;
simontruelove 16:e6c8df9960c6 342 case 26:Ph0();break;
simontruelove 14:1eb49362a607 343 case 27:Ph2();break;
simontruelove 14:1eb49362a607 344 case 28:Ph2();break;
simontruelove 16:e6c8df9960c6 345 case 29:Ph0();break;
simontruelove 14:1eb49362a607 346 case 30:Ph2();break;
simontruelove 16:e6c8df9960c6 347 case 31:Ph0();break;
simontruelove 14:1eb49362a607 348 case 32:Ph3();break;
simontruelove 14:1eb49362a607 349 case 33:Ph3();break;
simontruelove 14:1eb49362a607 350 case 34:Ph3();break;
simontruelove 14:1eb49362a607 351 case 35:Ph3();break;
simontruelove 14:1eb49362a607 352 case 36:Ph3();break;
simontruelove 14:1eb49362a607 353 case 37:Ph3();break;
simontruelove 14:1eb49362a607 354 case 38:Ph3();break;
simontruelove 14:1eb49362a607 355 case 39:Ph3();break;
simontruelove 14:1eb49362a607 356 case 40:Ph3();break;
simontruelove 14:1eb49362a607 357 case 41:Ph3();break;
simontruelove 16:e6c8df9960c6 358 case 42:Ph0();break;
simontruelove 14:1eb49362a607 359 case 43:Ph3();break;
simontruelove 14:1eb49362a607 360 case 44:Ph3();break;
simontruelove 16:e6c8df9960c6 361 case 45:Ph0();break;
simontruelove 14:1eb49362a607 362 case 46:Ph3();break;
simontruelove 16:e6c8df9960c6 363 case 47:Ph0();break;
simontruelove 14:1eb49362a607 364 case 48:Ph4();break;
simontruelove 14:1eb49362a607 365 case 49:Ph4();break;
simontruelove 14:1eb49362a607 366 case 50:Ph4();break;
simontruelove 14:1eb49362a607 367 case 51:Ph4();break;
simontruelove 14:1eb49362a607 368 case 52:Ph4();break;
simontruelove 14:1eb49362a607 369 case 53:Ph4();break;
simontruelove 14:1eb49362a607 370 case 54:Ph4();break;
simontruelove 14:1eb49362a607 371 case 55:Ph4();break;
simontruelove 14:1eb49362a607 372 case 56:Ph4();break;
simontruelove 14:1eb49362a607 373 case 57:Ph4();break;
simontruelove 16:e6c8df9960c6 374 case 58:Ph0();break;
simontruelove 14:1eb49362a607 375 case 59:Ph4();break;
simontruelove 14:1eb49362a607 376 case 60:Ph4();break;
simontruelove 16:e6c8df9960c6 377 case 61:Ph0();break;
simontruelove 14:1eb49362a607 378 case 62:Ph4();break;
simontruelove 16:e6c8df9960c6 379 case 63:Ph0();break;
simontruelove 5:4e5c644d5cc3 380 default:break;
simontruelove 3:4249dbdf7ed3 381 }
simontruelove 20:dca9f4c12fe3 382
simontruelove 20:dca9f4c12fe3 383 if(wheel.getWhoop()==1)
simontruelove 5:4e5c644d5cc3 384 {
simontruelove 5:4e5c644d5cc3 385 RPM();
simontruelove 9:061600a6c750 386 VelocityLoop();
simontruelove 14:1eb49362a607 387 slowloop++;
simontruelove 16:e6c8df9960c6 388 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 389 {
simontruelove 14:1eb49362a607 390 ReadKType();
simontruelove 22:58558001804d 391 lcd.cls();
simontruelove 22:58558001804d 392 wait(0.0001);
simontruelove 22:58558001804d 393 lcd.printf("CW %.0f\n",rpm);
simontruelove 22:58558001804d 394 slowloop=0;
simontruelove 14:1eb49362a607 395 }
simontruelove 11:74eeb8871fe6 396 }
simontruelove 5:4e5c644d5cc3 397 }
simontruelove 22:58558001804d 398 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (anticlockwise==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
simontruelove 4:3aedc9246ae4 399 {
simontruelove 5:4e5c644d5cc3 400 GetChar();
simontruelove 22:58558001804d 401 Switch();
simontruelove 22:58558001804d 402 StateB = (wheel.getPulses()+StateA+AdjACW-adj)%s;
simontruelove 22:58558001804d 403 SetPointBuffer = (float) 3000 *pot;
simontruelove 22:58558001804d 404 SetPoint = (int) SetPointBuffer;
simontruelove 20:dca9f4c12fe3 405
simontruelove 5:4e5c644d5cc3 406 switch(StateB)
simontruelove 5:4e5c644d5cc3 407 {
simontruelove 14:1eb49362a607 408 case 63:Ph4();break;
simontruelove 14:1eb49362a607 409 case 62:Ph4();break;
simontruelove 14:1eb49362a607 410 case 61:Ph4();break;
simontruelove 14:1eb49362a607 411 case 60:Ph4();break;
simontruelove 14:1eb49362a607 412 case 59:Ph4();break;
simontruelove 14:1eb49362a607 413 case 58:Ph4();break;
simontruelove 14:1eb49362a607 414 case 57:Ph4();break;
simontruelove 14:1eb49362a607 415 case 56:Ph4();break;
simontruelove 14:1eb49362a607 416 case 55:Ph4();break;
simontruelove 14:1eb49362a607 417 case 54:Ph4();break;
simontruelove 16:e6c8df9960c6 418 case 53:Ph0();break;
simontruelove 14:1eb49362a607 419 case 52:Ph4();break;
simontruelove 14:1eb49362a607 420 case 51:Ph4();break;
simontruelove 16:e6c8df9960c6 421 case 50:Ph0();break;
simontruelove 14:1eb49362a607 422 case 49:Ph4();break;
simontruelove 16:e6c8df9960c6 423 case 48:Ph0();break;
simontruelove 14:1eb49362a607 424 case 47:Ph3();break;
simontruelove 14:1eb49362a607 425 case 46:Ph3();break;
simontruelove 14:1eb49362a607 426 case 45:Ph3();break;
simontruelove 14:1eb49362a607 427 case 44:Ph3();break;
simontruelove 14:1eb49362a607 428 case 43:Ph3();break;
simontruelove 14:1eb49362a607 429 case 42:Ph3();break;
simontruelove 14:1eb49362a607 430 case 41:Ph3();break;
simontruelove 14:1eb49362a607 431 case 40:Ph3();break;
simontruelove 14:1eb49362a607 432 case 39:Ph3();break;
simontruelove 14:1eb49362a607 433 case 38:Ph3();break;
simontruelove 16:e6c8df9960c6 434 case 37:Ph0();break;
simontruelove 14:1eb49362a607 435 case 36:Ph3();break;
simontruelove 14:1eb49362a607 436 case 35:Ph3();break;
simontruelove 16:e6c8df9960c6 437 case 34:Ph0();break;
simontruelove 14:1eb49362a607 438 case 33:Ph3();break;
simontruelove 16:e6c8df9960c6 439 case 32:Ph0();break;
simontruelove 14:1eb49362a607 440 case 31:Ph2();break;
simontruelove 14:1eb49362a607 441 case 30:Ph2();break;
simontruelove 14:1eb49362a607 442 case 29:Ph2();break;
simontruelove 14:1eb49362a607 443 case 28:Ph2();break;
simontruelove 14:1eb49362a607 444 case 27:Ph2();break;
simontruelove 14:1eb49362a607 445 case 26:Ph2();break;
simontruelove 14:1eb49362a607 446 case 25:Ph2();break;
simontruelove 14:1eb49362a607 447 case 24:Ph2();break;
simontruelove 14:1eb49362a607 448 case 23:Ph2();break;
simontruelove 14:1eb49362a607 449 case 22:Ph2();break;
simontruelove 16:e6c8df9960c6 450 case 21:Ph0();break;
simontruelove 14:1eb49362a607 451 case 20:Ph2();break;
simontruelove 14:1eb49362a607 452 case 19:Ph2();break;
simontruelove 16:e6c8df9960c6 453 case 18:Ph0();break;
simontruelove 14:1eb49362a607 454 case 17:Ph2();break;
simontruelove 16:e6c8df9960c6 455 case 16:Ph0();break;
simontruelove 14:1eb49362a607 456 case 15:Ph1();break;
simontruelove 14:1eb49362a607 457 case 14:Ph1();break;
simontruelove 14:1eb49362a607 458 case 13:Ph1();break;
simontruelove 14:1eb49362a607 459 case 12:Ph1();break;
simontruelove 14:1eb49362a607 460 case 11:Ph1();break;
simontruelove 14:1eb49362a607 461 case 10:Ph1();break;
simontruelove 14:1eb49362a607 462 case 9:Ph1();break;
simontruelove 14:1eb49362a607 463 case 8:Ph1();break;
simontruelove 14:1eb49362a607 464 case 7:Ph1();break;
simontruelove 14:1eb49362a607 465 case 6:Ph1();break;
simontruelove 16:e6c8df9960c6 466 case 5:Ph0();break;
simontruelove 14:1eb49362a607 467 case 4:Ph1();break;
simontruelove 13:da9d3fbbe407 468 case 3:Ph1();break;
simontruelove 16:e6c8df9960c6 469 case 2:Ph0();break;
simontruelove 6:f7028034aabb 470 case 1:Ph1();break;
simontruelove 16:e6c8df9960c6 471 case 0:Ph0();break;
simontruelove 5:4e5c644d5cc3 472 default:break;
simontruelove 20:dca9f4c12fe3 473 }
simontruelove 20:dca9f4c12fe3 474 if(wheel.getWhoop()==1)
simontruelove 5:4e5c644d5cc3 475 {
simontruelove 5:4e5c644d5cc3 476 RPM();
simontruelove 9:061600a6c750 477 VelocityLoop();
simontruelove 14:1eb49362a607 478 slowloop++;
simontruelove 16:e6c8df9960c6 479 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 480 {
simontruelove 14:1eb49362a607 481 ReadKType();
simontruelove 22:58558001804d 482 lcd.cls();
simontruelove 22:58558001804d 483 wait(0.0001);
simontruelove 22:58558001804d 484 lcd.printf("ACW %.0f\n",rpm);
simontruelove 14:1eb49362a607 485 slowloop=0;
simontruelove 14:1eb49362a607 486 }
simontruelove 5:4e5c644d5cc3 487 }
simontruelove 3:4249dbdf7ed3 488 }
simontruelove 22:58558001804d 489 while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (anticlockwise==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
simontruelove 5:4e5c644d5cc3 490 {
simontruelove 5:4e5c644d5cc3 491 GetChar();
simontruelove 22:58558001804d 492 Switch();
simontruelove 22:58558001804d 493 StateB = (enc+wheel.getPulses()+StateA+AdjACW-adj)%s;
simontruelove 22:58558001804d 494 SetPointBuffer = (float) 3000 *pot;
simontruelove 22:58558001804d 495 SetPoint = (int) SetPointBuffer;
simontruelove 20:dca9f4c12fe3 496
simontruelove 5:4e5c644d5cc3 497 switch(StateB)
simontruelove 5:4e5c644d5cc3 498 {
simontruelove 14:1eb49362a607 499 case 63:Ph4();break;
simontruelove 14:1eb49362a607 500 case 62:Ph4();break;
simontruelove 14:1eb49362a607 501 case 61:Ph4();break;
simontruelove 14:1eb49362a607 502 case 60:Ph4();break;
simontruelove 14:1eb49362a607 503 case 59:Ph4();break;
simontruelove 14:1eb49362a607 504 case 58:Ph4();break;
simontruelove 14:1eb49362a607 505 case 57:Ph4();break;
simontruelove 14:1eb49362a607 506 case 56:Ph4();break;
simontruelove 14:1eb49362a607 507 case 55:Ph4();break;
simontruelove 14:1eb49362a607 508 case 54:Ph4();break;
simontruelove 16:e6c8df9960c6 509 case 53:Ph0();break;
simontruelove 14:1eb49362a607 510 case 52:Ph4();break;
simontruelove 14:1eb49362a607 511 case 51:Ph4();break;
simontruelove 16:e6c8df9960c6 512 case 50:Ph0();break;
simontruelove 14:1eb49362a607 513 case 49:Ph4();break;
simontruelove 16:e6c8df9960c6 514 case 48:Ph0();break;
simontruelove 14:1eb49362a607 515 case 47:Ph3();break;
simontruelove 14:1eb49362a607 516 case 46:Ph3();break;
simontruelove 14:1eb49362a607 517 case 45:Ph3();break;
simontruelove 14:1eb49362a607 518 case 44:Ph3();break;
simontruelove 14:1eb49362a607 519 case 43:Ph3();break;
simontruelove 14:1eb49362a607 520 case 42:Ph3();break;
simontruelove 14:1eb49362a607 521 case 41:Ph3();break;
simontruelove 14:1eb49362a607 522 case 40:Ph3();break;
simontruelove 14:1eb49362a607 523 case 39:Ph3();break;
simontruelove 14:1eb49362a607 524 case 38:Ph3();break;
simontruelove 16:e6c8df9960c6 525 case 37:Ph0();break;
simontruelove 14:1eb49362a607 526 case 36:Ph3();break;
simontruelove 14:1eb49362a607 527 case 35:Ph3();break;
simontruelove 16:e6c8df9960c6 528 case 34:Ph0();break;
simontruelove 14:1eb49362a607 529 case 33:Ph3();break;
simontruelove 16:e6c8df9960c6 530 case 32:Ph0();break;
simontruelove 14:1eb49362a607 531 case 31:Ph2();break;
simontruelove 14:1eb49362a607 532 case 30:Ph2();break;
simontruelove 14:1eb49362a607 533 case 29:Ph2();break;
simontruelove 14:1eb49362a607 534 case 28:Ph2();break;
simontruelove 14:1eb49362a607 535 case 27:Ph2();break;
simontruelove 14:1eb49362a607 536 case 26:Ph2();break;
simontruelove 14:1eb49362a607 537 case 25:Ph2();break;
simontruelove 14:1eb49362a607 538 case 24:Ph2();break;
simontruelove 14:1eb49362a607 539 case 23:Ph2();break;
simontruelove 14:1eb49362a607 540 case 22:Ph2();break;
simontruelove 16:e6c8df9960c6 541 case 21:Ph0();break;
simontruelove 14:1eb49362a607 542 case 20:Ph2();break;
simontruelove 14:1eb49362a607 543 case 19:Ph2();break;
simontruelove 16:e6c8df9960c6 544 case 18:Ph0();break;
simontruelove 14:1eb49362a607 545 case 17:Ph2();break;
simontruelove 16:e6c8df9960c6 546 case 16:Ph0();break;
simontruelove 14:1eb49362a607 547 case 15:Ph1();break;
simontruelove 14:1eb49362a607 548 case 14:Ph1();break;
simontruelove 14:1eb49362a607 549 case 13:Ph1();break;
simontruelove 14:1eb49362a607 550 case 12:Ph1();break;
simontruelove 14:1eb49362a607 551 case 11:Ph1();break;
simontruelove 14:1eb49362a607 552 case 10:Ph1();break;
simontruelove 14:1eb49362a607 553 case 9:Ph1();break;
simontruelove 14:1eb49362a607 554 case 8:Ph1();break;
simontruelove 14:1eb49362a607 555 case 7:Ph1();break;
simontruelove 14:1eb49362a607 556 case 6:Ph1();break;
simontruelove 16:e6c8df9960c6 557 case 5:Ph0();break;
simontruelove 14:1eb49362a607 558 case 4:Ph1();break;
simontruelove 13:da9d3fbbe407 559 case 3:Ph1();break;
simontruelove 16:e6c8df9960c6 560 case 2:Ph0();break;
simontruelove 6:f7028034aabb 561 case 1:Ph1();break;
simontruelove 16:e6c8df9960c6 562 case 0:Ph0();break;
simontruelove 14:1eb49362a607 563 default:break;
simontruelove 5:4e5c644d5cc3 564 }
simontruelove 20:dca9f4c12fe3 565
simontruelove 20:dca9f4c12fe3 566 if(wheel.getWhoop()==1)
simontruelove 5:4e5c644d5cc3 567 {
simontruelove 5:4e5c644d5cc3 568 RPM();
simontruelove 9:061600a6c750 569 VelocityLoop();
simontruelove 14:1eb49362a607 570 slowloop++;
simontruelove 16:e6c8df9960c6 571 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 572 {
simontruelove 14:1eb49362a607 573 ReadKType();
simontruelove 22:58558001804d 574 lcd.cls();
simontruelove 22:58558001804d 575 wait(0.0001);
simontruelove 22:58558001804d 576 lcd.printf("CW %.0f\n",rpm);
simontruelove 22:58558001804d 577 slowloop=0;
simontruelove 14:1eb49362a607 578 }
simontruelove 11:74eeb8871fe6 579 }
simontruelove 5:4e5c644d5cc3 580 }
simontruelove 20:dca9f4c12fe3 581 while(temp>(T-1)) //If Temp exceeds T the motor will not operate
simontruelove 11:74eeb8871fe6 582 {
simontruelove 11:74eeb8871fe6 583 Initialisation();
simontruelove 22:58558001804d 584 wait(0.001);
simontruelove 22:58558001804d 585 lcd.printf("SRM Over Temp");
simontruelove 22:58558001804d 586 lcd.locate(0,1);
simontruelove 22:58558001804d 587 lcd.printf("RPM=%.0f, %.0f C\n",rpm, temp);
simontruelove 22:58558001804d 588 wait(0.1);
simontruelove 11:74eeb8871fe6 589 pc.printf("Motor Over Temp\n\r");
simontruelove 22:58558001804d 590 while(1)
simontruelove 11:74eeb8871fe6 591 {
simontruelove 20:dca9f4c12fe3 592 ReadKType(); //Read motor Temp
simontruelove 22:58558001804d 593 lcd.printf("SRM Over Temp");
simontruelove 22:58558001804d 594 lcd.locate(0,1);
simontruelove 22:58558001804d 595 lcd.printf("RPM=%.0f, %.0f C\n",rpm, temp);
simontruelove 22:58558001804d 596 wait(0.1);
simontruelove 20:dca9f4c12fe3 597 pc.printf("%f\r",temp); //Display current Temp
simontruelove 20:dca9f4c12fe3 598 wait(1);
simontruelove 20:dca9f4c12fe3 599 if(temp<T-20) //Reset when Temp 20 degrees C below thermostat trip value
simontruelove 20:dca9f4c12fe3 600 {
simontruelove 22:58558001804d 601 wait(0.001);
simontruelove 22:58558001804d 602 lcd.printf("SRM Back Online");
simontruelove 22:58558001804d 603 lcd.locate(0,1);
simontruelove 22:58558001804d 604 lcd.printf("RPM=%.0f, %.0f C\n",rpm, temp);
simontruelove 22:58558001804d 605 wait(0.1);
simontruelove 20:dca9f4c12fe3 606 pc.printf("Motor Back Online\n\r");
simontruelove 20:dca9f4c12fe3 607 break;
simontruelove 11:74eeb8871fe6 608 }
simontruelove 20:dca9f4c12fe3 609 }
simontruelove 11:74eeb8871fe6 610 }
simontruelove 1:0191658b6ff4 611 }
simontruelove 1:0191658b6ff4 612 }
simontruelove 16:e6c8df9960c6 613 void StepACW(void) //Square wave switching
simontruelove 1:0191658b6ff4 614 {
simontruelove 1:0191658b6ff4 615 Ph4();
simontruelove 3:4249dbdf7ed3 616 wait(x);
simontruelove 18:3493de6fe8ce 617 pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 618 //Ph34();
simontruelove 18:3493de6fe8ce 619 //wait(y);
simontruelove 14:1eb49362a607 620 //pc.printf("34 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 621 Ph3();
simontruelove 14:1eb49362a607 622 wait(x);
simontruelove 18:3493de6fe8ce 623 pc.printf("3 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 624 //Ph23();
simontruelove 18:3493de6fe8ce 625 //wait(y);
simontruelove 14:1eb49362a607 626 //pc.printf("23 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 627 Ph2();
simontruelove 14:1eb49362a607 628 wait(x);
simontruelove 18:3493de6fe8ce 629 pc.printf("2 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 630 //Ph12();
simontruelove 18:3493de6fe8ce 631 //wait(y);
simontruelove 14:1eb49362a607 632 //pc.printf("12 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 633 Ph1();
simontruelove 14:1eb49362a607 634 wait(x);
simontruelove 18:3493de6fe8ce 635 pc.printf("1 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 636 //Ph41();
simontruelove 18:3493de6fe8ce 637 // wait(y);
simontruelove 14:1eb49362a607 638 //pc.printf("41 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 13:da9d3fbbe407 639 }
simontruelove 13:da9d3fbbe407 640
simontruelove 20:dca9f4c12fe3 641 void Ph0(void) //Turn off all Phases
simontruelove 13:da9d3fbbe407 642 {
simontruelove 13:da9d3fbbe407 643 Phase1.write(0);
simontruelove 13:da9d3fbbe407 644 Phase2.write(0);
simontruelove 13:da9d3fbbe407 645 Phase3.write(0);
simontruelove 13:da9d3fbbe407 646 Phase4.write(0);
simontruelove 1:0191658b6ff4 647 }
simontruelove 1:0191658b6ff4 648
simontruelove 20:dca9f4c12fe3 649 void Ph1(void) //Turn on Phase 1
simontruelove 1:0191658b6ff4 650 {
simontruelove 9:061600a6c750 651 Phase1.write(duty);
simontruelove 9:061600a6c750 652 Phase2.write(0);
simontruelove 9:061600a6c750 653 Phase3.write(0);
simontruelove 9:061600a6c750 654 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 655 }
simontruelove 3:4249dbdf7ed3 656
simontruelove 20:dca9f4c12fe3 657 void Ph12 (void) //Turn on Phase 1 and 2
simontruelove 3:4249dbdf7ed3 658 {
simontruelove 9:061600a6c750 659 Phase1.write(duty);
simontruelove 9:061600a6c750 660 Phase2.write(duty);
simontruelove 9:061600a6c750 661 Phase3.write(0);
simontruelove 9:061600a6c750 662 Phase4.write(0);
simontruelove 1:0191658b6ff4 663 }
simontruelove 1:0191658b6ff4 664
simontruelove 20:dca9f4c12fe3 665 void Ph2(void) //Turn on Phase 2
simontruelove 1:0191658b6ff4 666 {
simontruelove 9:061600a6c750 667 Phase1.write(0);
simontruelove 9:061600a6c750 668 Phase2.write(duty);
simontruelove 9:061600a6c750 669 Phase3.write(0);
simontruelove 9:061600a6c750 670 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 671 }
simontruelove 3:4249dbdf7ed3 672
simontruelove 20:dca9f4c12fe3 673 void Ph23 (void) //Turn on Phase 2 and 3
simontruelove 3:4249dbdf7ed3 674 {
simontruelove 9:061600a6c750 675 Phase1.write(0);
simontruelove 9:061600a6c750 676 Phase2.write(duty);
simontruelove 9:061600a6c750 677 Phase3.write(duty);
simontruelove 9:061600a6c750 678 Phase4.write(0);
simontruelove 1:0191658b6ff4 679 }
simontruelove 0:634dd505dace 680
simontruelove 20:dca9f4c12fe3 681 void Ph3(void) //Turn on Phase 3
simontruelove 1:0191658b6ff4 682 {
simontruelove 9:061600a6c750 683 Phase1.write(0);
simontruelove 9:061600a6c750 684 Phase2.write(0);
simontruelove 9:061600a6c750 685 Phase3.write(duty);
simontruelove 9:061600a6c750 686 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 687 }
simontruelove 20:dca9f4c12fe3 688
simontruelove 20:dca9f4c12fe3 689 void Ph34 (void) //Turn on Phase 3 and 4
simontruelove 3:4249dbdf7ed3 690 {
simontruelove 9:061600a6c750 691 Phase1.write(0);
simontruelove 9:061600a6c750 692 Phase2.write(0);
simontruelove 9:061600a6c750 693 Phase3.write(duty);
simontruelove 9:061600a6c750 694 Phase4.write(duty);
simontruelove 1:0191658b6ff4 695 }
simontruelove 1:0191658b6ff4 696
simontruelove 20:dca9f4c12fe3 697 void Ph4(void) //Turn on Phase 4
simontruelove 1:0191658b6ff4 698 {
simontruelove 9:061600a6c750 699 Phase1.write(0);
simontruelove 9:061600a6c750 700 Phase2.write(0);
simontruelove 9:061600a6c750 701 Phase3.write(0);
simontruelove 9:061600a6c750 702 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 703 }
simontruelove 3:4249dbdf7ed3 704
simontruelove 20:dca9f4c12fe3 705 void Ph41 (void) //Turn on Phase 4 and 1
simontruelove 3:4249dbdf7ed3 706 {
simontruelove 9:061600a6c750 707 Phase1.write(duty);
simontruelove 9:061600a6c750 708 Phase2.write(0);
simontruelove 9:061600a6c750 709 Phase3.write(0);
simontruelove 9:061600a6c750 710 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 711 }
simontruelove 3:4249dbdf7ed3 712
simontruelove 20:dca9f4c12fe3 713 void Initialisation (void) //Turn everything off
simontruelove 3:4249dbdf7ed3 714 {
simontruelove 9:061600a6c750 715 Phase1.write(0);
simontruelove 9:061600a6c750 716 Phase2.write(0);
simontruelove 9:061600a6c750 717 Phase3.write(0);
simontruelove 9:061600a6c750 718 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 719 led1 = 0;
simontruelove 3:4249dbdf7ed3 720 led2 = 0;
simontruelove 3:4249dbdf7ed3 721 led3 = 0;
simontruelove 3:4249dbdf7ed3 722 led4 = 0;
simontruelove 12:cbea987a3ec4 723 UnUsedPhase1=0;
simontruelove 12:cbea987a3ec4 724 UnUsedPhase2=0;
simontruelove 3:4249dbdf7ed3 725 wheel.ResetYay();
simontruelove 14:1eb49362a607 726 wheel.QEI::reset();
simontruelove 4:3aedc9246ae4 727 }
simontruelove 4:3aedc9246ae4 728
simontruelove 20:dca9f4c12fe3 729 void GetChar (void) //read keyboard strikes with terraterm
simontruelove 4:3aedc9246ae4 730 { if (pc.readable())
simontruelove 4:3aedc9246ae4 731 {
simontruelove 4:3aedc9246ae4 732 c = pc.getc();
simontruelove 20:dca9f4c12fe3 733 switch(c)
simontruelove 14:1eb49362a607 734 {
simontruelove 20:dca9f4c12fe3 735 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;
simontruelove 20:dca9f4c12fe3 736 case '1': Ph1();break; //Check Phase 1
simontruelove 20:dca9f4c12fe3 737 case '2': Ph2();break; //Check Phase 2
simontruelove 20:dca9f4c12fe3 738 case '3': Ph3();break; //Check Phase 3
simontruelove 20:dca9f4c12fe3 739 case '4': Ph4();break; //Check Phase 4
simontruelove 20:dca9f4c12fe3 740 case '5': Ph0();break; //Turn off all Phases
simontruelove 20:dca9f4c12fe3 741 case '6': StepACW();break; //Step ACW
simontruelove 22:58558001804d 742 case '7': pc.printf("TimePerClick = %i, TimePerRev = %.5f, rps = %.5f, rpm = %.1f, \n\r", TimePerClick, TimePerRev, rps, rpm);break; //Print rpm related data
simontruelove 22:58558001804d 743 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
simontruelove 22:58558001804d 744 case '0': pc.printf("%i, %.5f, %i, %i, %.1f, %f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, gain, p);break; //Print general data
simontruelove 22:58558001804d 745 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
simontruelove 22:58558001804d 746 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
simontruelove 22:58558001804d 747 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
simontruelove 22:58558001804d 748 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
simontruelove 22:58558001804d 749 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
simontruelove 22:58558001804d 750 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
simontruelove 22:58558001804d 751 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
simontruelove 20:dca9f4c12fe3 752 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
simontruelove 22:58558001804d 753 case 't': pc.printf("%.0f C\n\r",temp);break; //Print current Temp
simontruelove 22:58558001804d 754 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+
simontruelove 22:58558001804d 755 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-
simontruelove 22:58558001804d 756 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+
simontruelove 22:58558001804d 757 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-
simontruelove 22:58558001804d 758 case 'z': led1 = !led1;led2 = 0;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Run motor CW
simontruelove 22:58558001804d 759 case 'x': led1 = 0;led2 = !led2;pc.printf("%i, %.5f, %i, %i, %.1f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);break; //Run motor ACW
simontruelove 22:58558001804d 760 case 'm': pc.printf("%f \n\r", pot.read()); //Potentiometer value
simontruelove 14:1eb49362a607 761 }
simontruelove 4:3aedc9246ae4 762 }
simontruelove 5:4e5c644d5cc3 763 }
simontruelove 5:4e5c644d5cc3 764
simontruelove 5:4e5c644d5cc3 765 void RPM (void)
simontruelove 5:4e5c644d5cc3 766 {
simontruelove 17:19b2c598810a 767 wheel.ResetWhoop(); //Whoop = 1 x per rev
simontruelove 14:1eb49362a607 768 TimePerClick = (t.read_us()); //read timer in microseconds
simontruelove 14:1eb49362a607 769 t.reset(); //reset timer
simontruelove 20:dca9f4c12fe3 770 TimePerRev = TimePerClick; //Convert from int to float TimePerClick = int, TimePerRev = float
simontruelove 14:1eb49362a607 771 TimePerRev = TimePerRev / 1000000; // 1microsecond = 0.000001s
simontruelove 20:dca9f4c12fe3 772 rps = 1 / TimePerRev; //inverse to convert SPR to rps
simontruelove 20:dca9f4c12fe3 773 rpm = rps * 60; //x 60 to convert rps to RPM
simontruelove 20:dca9f4c12fe3 774
simontruelove 22:58558001804d 775 //if(duty < 0.9) //Mapped Phase Advance with no load
simontruelove 20:dca9f4c12fe3 776 {
simontruelove 22:58558001804d 777 if (rpm<299)
simontruelove 22:58558001804d 778 {
simontruelove 22:58558001804d 779 AdjCW = (57);
simontruelove 22:58558001804d 780 AdjACW = (12);
simontruelove 22:58558001804d 781 gain = (0.0001);
simontruelove 22:58558001804d 782 }
simontruelove 22:58558001804d 783 else if(rpm > 299 and rpm < 309)
simontruelove 22:58558001804d 784 {
simontruelove 22:58558001804d 785 AdjCW = (59);
simontruelove 22:58558001804d 786 AdjACW = (12);
simontruelove 22:58558001804d 787 gain = (0.0001);
simontruelove 22:58558001804d 788 }
simontruelove 22:58558001804d 789 else if(rpm > 309 and rpm < 319)
simontruelove 22:58558001804d 790 {
simontruelove 22:58558001804d 791 AdjCW = (60);
simontruelove 22:58558001804d 792 AdjACW = (12);
simontruelove 22:58558001804d 793 gain = (0.0001);
simontruelove 22:58558001804d 794 }
simontruelove 22:58558001804d 795 else if(rpm > 319 and rpm < 328)
simontruelove 22:58558001804d 796 {
simontruelove 22:58558001804d 797 AdjCW = (61);
simontruelove 22:58558001804d 798 AdjACW = (12);
simontruelove 22:58558001804d 799 gain = (0.0001);
simontruelove 22:58558001804d 800 }
simontruelove 22:58558001804d 801 else if(rpm > 328 and rpm < 337)
simontruelove 22:58558001804d 802 {
simontruelove 22:58558001804d 803 AdjCW = (62);
simontruelove 22:58558001804d 804 AdjACW = (12);
simontruelove 22:58558001804d 805 gain = (0.0001);
simontruelove 22:58558001804d 806 }
simontruelove 22:58558001804d 807 else if(rpm > 337 and rpm < 346)
simontruelove 22:58558001804d 808 {
simontruelove 22:58558001804d 809 AdjCW = (63);
simontruelove 22:58558001804d 810 AdjACW = (12);
simontruelove 22:58558001804d 811 gain = (0.0001);
simontruelove 22:58558001804d 812 }
simontruelove 22:58558001804d 813 else if(rpm > 346 and rpm < 351)
simontruelove 20:dca9f4c12fe3 814 {
simontruelove 21:b831f68ce5ed 815 AdjCW = (0);
simontruelove 22:58558001804d 816 AdjACW = (12);
simontruelove 22:58558001804d 817 gain = (0.0001);
simontruelove 22:58558001804d 818 }
simontruelove 22:58558001804d 819 else if(rpm > 351 and rpm < 357)
simontruelove 22:58558001804d 820 {
simontruelove 22:58558001804d 821 AdjCW = (1);
simontruelove 22:58558001804d 822 AdjACW = (11);
simontruelove 22:58558001804d 823 gain = (0.0001);
simontruelove 22:58558001804d 824 }
simontruelove 22:58558001804d 825 else if(rpm > 357 and rpm < 362)
simontruelove 22:58558001804d 826 {
simontruelove 22:58558001804d 827 AdjCW = (3);
simontruelove 22:58558001804d 828 AdjACW = (10);
simontruelove 22:58558001804d 829 gain = (0.0001);
simontruelove 22:58558001804d 830 }
simontruelove 22:58558001804d 831 else if(rpm > 362 and rpm < 420)
simontruelove 22:58558001804d 832 {
simontruelove 22:58558001804d 833 AdjCW = (4);
simontruelove 22:58558001804d 834 AdjACW = (6);
simontruelove 22:58558001804d 835 gain = (0.0001);
simontruelove 22:58558001804d 836 }
simontruelove 22:58558001804d 837 else if(rpm > 420 and rpm < 548)
simontruelove 22:58558001804d 838 {
simontruelove 22:58558001804d 839 AdjCW = (5);
simontruelove 22:58558001804d 840 AdjACW = (4);
simontruelove 22:58558001804d 841 gain = (0.0001);
simontruelove 22:58558001804d 842 }
simontruelove 22:58558001804d 843 else if(rpm > 548 and rpm < 664)
simontruelove 22:58558001804d 844 {
simontruelove 22:58558001804d 845 AdjCW = (6);
simontruelove 22:58558001804d 846 AdjACW = (3);
simontruelove 22:58558001804d 847 gain = (0.0001);
simontruelove 22:58558001804d 848 }
simontruelove 22:58558001804d 849 else if(rpm > 664 and rpm < 710)
simontruelove 22:58558001804d 850 {
simontruelove 22:58558001804d 851 AdjCW = (7);
simontruelove 22:58558001804d 852 AdjACW = (3);
simontruelove 22:58558001804d 853 gain = (0.0001);
simontruelove 22:58558001804d 854 }
simontruelove 22:58558001804d 855 else if(rpm > 710 and rpm < 767)
simontruelove 22:58558001804d 856 {
simontruelove 22:58558001804d 857 AdjCW = (8);
simontruelove 22:58558001804d 858 AdjACW = (2);
simontruelove 22:58558001804d 859 gain = (0.0001);
simontruelove 22:58558001804d 860 }
simontruelove 22:58558001804d 861 else if(rpm > 767 and rpm < 960)
simontruelove 22:58558001804d 862 {
simontruelove 22:58558001804d 863 AdjCW = (9);
simontruelove 22:58558001804d 864 AdjACW = (1);
simontruelove 22:58558001804d 865 gain = (0.0001);
simontruelove 22:58558001804d 866 }
simontruelove 22:58558001804d 867 else if(rpm > 960 and rpm < 1051)
simontruelove 22:58558001804d 868 {
simontruelove 22:58558001804d 869 AdjCW = (10);
simontruelove 22:58558001804d 870 AdjACW = (0);
simontruelove 21:b831f68ce5ed 871 gain = (0.0001);
simontruelove 22:58558001804d 872 }
simontruelove 22:58558001804d 873 else if(rpm > 1051 and rpm < 1281)
simontruelove 22:58558001804d 874 {
simontruelove 22:58558001804d 875 AdjCW = (11);
simontruelove 22:58558001804d 876 AdjACW = (63);
simontruelove 22:58558001804d 877 gain = (0.00001);
simontruelove 22:58558001804d 878 }
simontruelove 22:58558001804d 879 else if(rpm > 1281 and rpm < 1427)
simontruelove 22:58558001804d 880 {
simontruelove 22:58558001804d 881 AdjCW = (12);
simontruelove 22:58558001804d 882 AdjACW = (62);
simontruelove 22:58558001804d 883 gain = (0.00001);
simontruelove 22:58558001804d 884 }
simontruelove 22:58558001804d 885 else if(rpm > 1427 and rpm < 1613)
simontruelove 22:58558001804d 886 {
simontruelove 22:58558001804d 887 AdjCW = (13);
simontruelove 22:58558001804d 888 AdjACW = (61);
simontruelove 22:58558001804d 889 gain = (0.00001);
simontruelove 22:58558001804d 890 }
simontruelove 22:58558001804d 891 else if(rpm > 1613 and rpm < 1661)
simontruelove 22:58558001804d 892 {
simontruelove 22:58558001804d 893 AdjCW = (14);
simontruelove 22:58558001804d 894 AdjACW = (60);
simontruelove 22:58558001804d 895 gain = (0.00001);
simontruelove 22:58558001804d 896 }
simontruelove 22:58558001804d 897 else if(rpm > 1661 and rpm < 1746)
simontruelove 22:58558001804d 898 {
simontruelove 22:58558001804d 899 AdjCW = (15);
simontruelove 22:58558001804d 900 AdjACW = (59);
simontruelove 22:58558001804d 901 gain = (0.00001);
simontruelove 22:58558001804d 902 }
simontruelove 22:58558001804d 903 else if(rpm > 1746 and rpm < 1999)
simontruelove 22:58558001804d 904 {
simontruelove 22:58558001804d 905 AdjCW = (16);
simontruelove 22:58558001804d 906 AdjACW = (58);
simontruelove 22:58558001804d 907 gain = (0.00001);
simontruelove 22:58558001804d 908 }
simontruelove 22:58558001804d 909 else if(rpm > 1999 and rpm < 2324)
simontruelove 22:58558001804d 910 {
simontruelove 22:58558001804d 911 AdjCW = (17);
simontruelove 22:58558001804d 912 AdjACW = (57);
simontruelove 22:58558001804d 913 gain = (0.00001);
simontruelove 22:58558001804d 914 }
simontruelove 22:58558001804d 915 else if(rpm > 2324 and rpm < 2621)
simontruelove 22:58558001804d 916 {
simontruelove 22:58558001804d 917 AdjCW = (18);
simontruelove 22:58558001804d 918 AdjACW = (56);
simontruelove 22:58558001804d 919 gain = (0.00001);
simontruelove 22:58558001804d 920 }
simontruelove 22:58558001804d 921 else if(rpm > 2621 and rpm < 2779)
simontruelove 22:58558001804d 922 {
simontruelove 22:58558001804d 923 AdjCW = (19);
simontruelove 22:58558001804d 924 AdjACW = (55);
simontruelove 22:58558001804d 925 gain = (0.00001);
simontruelove 22:58558001804d 926 }
simontruelove 22:58558001804d 927 else if(rpm > 2779 and rpm < 2882)
simontruelove 22:58558001804d 928 {
simontruelove 22:58558001804d 929 AdjCW = (20);
simontruelove 22:58558001804d 930 AdjACW = (54);
simontruelove 22:58558001804d 931 gain = (0.00001);
simontruelove 22:58558001804d 932 }
simontruelove 22:58558001804d 933 else if(rpm > 2882 and rpm < 2967)
simontruelove 22:58558001804d 934 {
simontruelove 22:58558001804d 935 AdjCW = (22);
simontruelove 22:58558001804d 936 AdjACW = (53);
simontruelove 22:58558001804d 937 gain = (0.000005);
simontruelove 22:58558001804d 938 }
simontruelove 22:58558001804d 939 else if(rpm > 2967)
simontruelove 22:58558001804d 940 {
simontruelove 22:58558001804d 941 AdjCW = (23);
simontruelove 22:58558001804d 942 AdjACW = (52);
simontruelove 22:58558001804d 943 gain = (0.000005);
simontruelove 22:58558001804d 944 }
simontruelove 22:58558001804d 945 }
simontruelove 22:58558001804d 946 /*if(rpm < 135)
simontruelove 22:58558001804d 947 {
simontruelove 22:58558001804d 948 AdjCW = (0);
simontruelove 22:58558001804d 949 AdjACW = (13);
simontruelove 22:58558001804d 950 gain = (0.0001); //proportional gain
simontruelove 20:dca9f4c12fe3 951 }
simontruelove 21:b831f68ce5ed 952 else if(rpm > 135.2 and rpm < 171)
simontruelove 20:dca9f4c12fe3 953 {
simontruelove 21:b831f68ce5ed 954 AdjCW = (1);
simontruelove 21:b831f68ce5ed 955 AdjACW = (13);
simontruelove 21:b831f68ce5ed 956 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 957 }
simontruelove 21:b831f68ce5ed 958 else if(rpm > 171 and rpm < 188)
simontruelove 20:dca9f4c12fe3 959 {
simontruelove 21:b831f68ce5ed 960 AdjCW = (1);
simontruelove 21:b831f68ce5ed 961 AdjACW = (11);
simontruelove 21:b831f68ce5ed 962 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 963 }
simontruelove 21:b831f68ce5ed 964 else if(rpm > 188 and rpm < 245)
simontruelove 20:dca9f4c12fe3 965 {
simontruelove 21:b831f68ce5ed 966 AdjCW = (2);
simontruelove 21:b831f68ce5ed 967 AdjACW = (11);
simontruelove 21:b831f68ce5ed 968 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 969 }
simontruelove 21:b831f68ce5ed 970 else if(rpm > 245 and rpm < 248)
simontruelove 20:dca9f4c12fe3 971 {
simontruelove 21:b831f68ce5ed 972 AdjCW = (2);
simontruelove 21:b831f68ce5ed 973 AdjACW = (10);
simontruelove 21:b831f68ce5ed 974 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 975 }
simontruelove 21:b831f68ce5ed 976 else if(rpm > 248 and rpm < 291)
simontruelove 20:dca9f4c12fe3 977 {
simontruelove 21:b831f68ce5ed 978 AdjCW = (3);
simontruelove 21:b831f68ce5ed 979 AdjACW = (10);
simontruelove 21:b831f68ce5ed 980 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 981 }
simontruelove 21:b831f68ce5ed 982 else if(rpm > 291 and rpm < 322)
simontruelove 20:dca9f4c12fe3 983 {
simontruelove 21:b831f68ce5ed 984 AdjCW = (3);
simontruelove 21:b831f68ce5ed 985 AdjACW = (9);
simontruelove 21:b831f68ce5ed 986 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 987 }
simontruelove 21:b831f68ce5ed 988 else if(rpm > 322 and rpm < 328)
simontruelove 20:dca9f4c12fe3 989 {
simontruelove 21:b831f68ce5ed 990 AdjCW = (3);
simontruelove 21:b831f68ce5ed 991 AdjACW = (8);
simontruelove 21:b831f68ce5ed 992 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 993 }
simontruelove 21:b831f68ce5ed 994 else if(rpm > 328 and rpm < 394)
simontruelove 20:dca9f4c12fe3 995 {
simontruelove 21:b831f68ce5ed 996 AdjCW = (4);
simontruelove 21:b831f68ce5ed 997 AdjACW = (8);
simontruelove 21:b831f68ce5ed 998 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 999 }
simontruelove 21:b831f68ce5ed 1000 else if(rpm > 394 and rpm < 435)
simontruelove 20:dca9f4c12fe3 1001 {
simontruelove 21:b831f68ce5ed 1002 AdjCW = (4);
simontruelove 21:b831f68ce5ed 1003 AdjACW = (7);
simontruelove 21:b831f68ce5ed 1004 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 1005 }
simontruelove 21:b831f68ce5ed 1006 else if(rpm > 435 and rpm < 459)
simontruelove 20:dca9f4c12fe3 1007 {
simontruelove 21:b831f68ce5ed 1008 AdjCW = (5);
simontruelove 21:b831f68ce5ed 1009 AdjACW = (7);
simontruelove 21:b831f68ce5ed 1010 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 1011 }
simontruelove 21:b831f68ce5ed 1012 else if(rpm > 459 and rpm < 512 )
simontruelove 20:dca9f4c12fe3 1013 {
simontruelove 21:b831f68ce5ed 1014 AdjCW = (5);
simontruelove 21:b831f68ce5ed 1015 AdjACW = (6);
simontruelove 21:b831f68ce5ed 1016 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 1017 }
simontruelove 21:b831f68ce5ed 1018 else if(rpm > 512 and rpm < 533)
simontruelove 20:dca9f4c12fe3 1019 {
simontruelove 21:b831f68ce5ed 1020 AdjCW = (6);
simontruelove 21:b831f68ce5ed 1021 AdjACW = (6);
simontruelove 21:b831f68ce5ed 1022 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 1023 }
simontruelove 21:b831f68ce5ed 1024 else if(rpm > 533 and rpm < 641)
simontruelove 20:dca9f4c12fe3 1025 {
simontruelove 21:b831f68ce5ed 1026 AdjCW = (6);
simontruelove 21:b831f68ce5ed 1027 AdjACW = (5);
simontruelove 21:b831f68ce5ed 1028 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 1029 }
simontruelove 21:b831f68ce5ed 1030 else if(rpm > 641 and rpm < 763)
simontruelove 20:dca9f4c12fe3 1031 {
simontruelove 21:b831f68ce5ed 1032 AdjCW = (7);
simontruelove 21:b831f68ce5ed 1033 AdjACW = (5);
simontruelove 21:b831f68ce5ed 1034 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 1035 }
simontruelove 21:b831f68ce5ed 1036 else if(rpm > 763 and rpm < 1002)
simontruelove 20:dca9f4c12fe3 1037 {
simontruelove 21:b831f68ce5ed 1038 AdjCW = (7);
simontruelove 21:b831f68ce5ed 1039 AdjACW = (4);
simontruelove 21:b831f68ce5ed 1040 gain = (0.0001);
simontruelove 20:dca9f4c12fe3 1041 }
simontruelove 21:b831f68ce5ed 1042 else if(rpm > 1002 and rpm < 1318)
simontruelove 20:dca9f4c12fe3 1043 {
simontruelove 21:b831f68ce5ed 1044 AdjCW = (8);
simontruelove 21:b831f68ce5ed 1045 AdjACW = (4);
simontruelove 21:b831f68ce5ed 1046 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1047 }
simontruelove 21:b831f68ce5ed 1048 else if(rpm > 1318 and rpm < 1338)
simontruelove 20:dca9f4c12fe3 1049 {
simontruelove 21:b831f68ce5ed 1050 AdjCW = (8);
simontruelove 21:b831f68ce5ed 1051 AdjACW = (3);
simontruelove 21:b831f68ce5ed 1052 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1053 }
simontruelove 21:b831f68ce5ed 1054 else if(rpm > 1338 and rpm < 1463)
simontruelove 20:dca9f4c12fe3 1055 {
simontruelove 21:b831f68ce5ed 1056 AdjCW = (9);
simontruelove 21:b831f68ce5ed 1057 AdjACW = (3);
simontruelove 21:b831f68ce5ed 1058 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1059 }
simontruelove 21:b831f68ce5ed 1060 else if(rpm > 1463 and rpm < 1493)
simontruelove 20:dca9f4c12fe3 1061 {
simontruelove 21:b831f68ce5ed 1062 AdjCW = (9);
simontruelove 21:b831f68ce5ed 1063 AdjACW = (2);
simontruelove 21:b831f68ce5ed 1064 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1065 }
simontruelove 21:b831f68ce5ed 1066 else if(rpm > 1493 and rpm < 1719)
simontruelove 20:dca9f4c12fe3 1067 {
simontruelove 21:b831f68ce5ed 1068 AdjCW = (10);
simontruelove 21:b831f68ce5ed 1069 AdjACW = (2);
simontruelove 21:b831f68ce5ed 1070 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1071 }
simontruelove 21:b831f68ce5ed 1072 else if(rpm > 1719 and rpm < 1780)
simontruelove 20:dca9f4c12fe3 1073 {
simontruelove 21:b831f68ce5ed 1074 AdjCW = (11);
simontruelove 21:b831f68ce5ed 1075 AdjACW = (2);
simontruelove 21:b831f68ce5ed 1076 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1077 }
simontruelove 21:b831f68ce5ed 1078 else if(rpm > 1780 and rpm < 2030)
simontruelove 20:dca9f4c12fe3 1079 {
simontruelove 21:b831f68ce5ed 1080 AdjCW = (11);
simontruelove 21:b831f68ce5ed 1081 AdjACW = (1);
simontruelove 21:b831f68ce5ed 1082 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1083 }
simontruelove 21:b831f68ce5ed 1084 else if(rpm > 2030 and rpm < 2073)
simontruelove 20:dca9f4c12fe3 1085 {
simontruelove 21:b831f68ce5ed 1086 AdjCW = (12);
simontruelove 21:b831f68ce5ed 1087 AdjACW = (1);
simontruelove 21:b831f68ce5ed 1088 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1089 }
simontruelove 21:b831f68ce5ed 1090 else if(rpm > 2073 and rpm < 2255)
simontruelove 20:dca9f4c12fe3 1091 {
simontruelove 21:b831f68ce5ed 1092 AdjCW = (12);
simontruelove 21:b831f68ce5ed 1093 AdjACW = (0);
simontruelove 21:b831f68ce5ed 1094 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1095 }
simontruelove 21:b831f68ce5ed 1096 else if(rpm > 2255 and rpm < 2463)
simontruelove 20:dca9f4c12fe3 1097 {
simontruelove 21:b831f68ce5ed 1098 AdjCW = (13);
simontruelove 21:b831f68ce5ed 1099 AdjACW = (0);
simontruelove 21:b831f68ce5ed 1100 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1101 }
simontruelove 21:b831f68ce5ed 1102 else if(rpm > 2463 and rpm < 2479)
simontruelove 20:dca9f4c12fe3 1103 {
simontruelove 21:b831f68ce5ed 1104 AdjCW = (14);
simontruelove 21:b831f68ce5ed 1105 AdjACW = (0);
simontruelove 21:b831f68ce5ed 1106 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1107 }
simontruelove 21:b831f68ce5ed 1108 else if(rpm > 2479 and rpm < 2591)
simontruelove 20:dca9f4c12fe3 1109 {
simontruelove 21:b831f68ce5ed 1110 AdjCW = (14);
simontruelove 21:b831f68ce5ed 1111 AdjACW = (63);
simontruelove 21:b831f68ce5ed 1112 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1113 }
simontruelove 21:b831f68ce5ed 1114 else if(rpm > 2591 and rpm < 2612)
simontruelove 20:dca9f4c12fe3 1115 {
simontruelove 21:b831f68ce5ed 1116 AdjCW = (14);
simontruelove 21:b831f68ce5ed 1117 AdjACW = (62);
simontruelove 21:b831f68ce5ed 1118 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1119 }
simontruelove 21:b831f68ce5ed 1120 else if(rpm > 2612 and rpm < 2742)
simontruelove 20:dca9f4c12fe3 1121 {
simontruelove 21:b831f68ce5ed 1122 AdjCW = (15);
simontruelove 21:b831f68ce5ed 1123 AdjACW = (62);
simontruelove 21:b831f68ce5ed 1124 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1125 }
simontruelove 21:b831f68ce5ed 1126 else if(rpm > 2742 and rpm < 2762)
simontruelove 20:dca9f4c12fe3 1127 {
simontruelove 21:b831f68ce5ed 1128 AdjCW = (16);
simontruelove 21:b831f68ce5ed 1129 AdjACW = (62);
simontruelove 21:b831f68ce5ed 1130 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1131 }
simontruelove 21:b831f68ce5ed 1132 else if(rpm > 2762 and rpm < 2894)
simontruelove 20:dca9f4c12fe3 1133 {
simontruelove 21:b831f68ce5ed 1134 AdjCW = (16);
simontruelove 21:b831f68ce5ed 1135 AdjACW = (61);
simontruelove 21:b831f68ce5ed 1136 gain = (0.00001);
simontruelove 20:dca9f4c12fe3 1137 }
simontruelove 21:b831f68ce5ed 1138 else if(rpm > 2894 and rpm < 2926)
simontruelove 20:dca9f4c12fe3 1139 {
simontruelove 21:b831f68ce5ed 1140 AdjCW = (17);
simontruelove 21:b831f68ce5ed 1141 AdjACW = (61);
simontruelove 21:b831f68ce5ed 1142 gain = (0.000001);
simontruelove 20:dca9f4c12fe3 1143 }
simontruelove 21:b831f68ce5ed 1144 else if(rpm > 2926 and rpm < 2982)
simontruelove 20:dca9f4c12fe3 1145 {
simontruelove 21:b831f68ce5ed 1146 AdjCW = (17);
simontruelove 21:b831f68ce5ed 1147 AdjACW = (59);
simontruelove 21:b831f68ce5ed 1148 gain = (0.000001);
simontruelove 20:dca9f4c12fe3 1149 }
simontruelove 21:b831f68ce5ed 1150 else if(rpm > 2982)
simontruelove 20:dca9f4c12fe3 1151 {
simontruelove 21:b831f68ce5ed 1152 AdjCW = (17);
simontruelove 21:b831f68ce5ed 1153 AdjACW = (57);
simontruelove 21:b831f68ce5ed 1154 gain = (0.000001);
simontruelove 22:58558001804d 1155 } */
simontruelove 22:58558001804d 1156 /*}
simontruelove 20:dca9f4c12fe3 1157
simontruelove 20:dca9f4c12fe3 1158 if(duty > 0.99) //Mapped Phase Advance with load
simontruelove 14:1eb49362a607 1159 {
simontruelove 19:d9ba2f225f39 1160 if(rpm < 48)
simontruelove 19:d9ba2f225f39 1161 {
simontruelove 19:d9ba2f225f39 1162 AdjCW = (52);
simontruelove 19:d9ba2f225f39 1163 AdjACW = (19);
simontruelove 21:b831f68ce5ed 1164 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1165 }
simontruelove 20:dca9f4c12fe3 1166 else if(rpm > 48 and rpm < 56)
simontruelove 19:d9ba2f225f39 1167 {
simontruelove 19:d9ba2f225f39 1168 AdjCW = (53);
simontruelove 19:d9ba2f225f39 1169 AdjACW = (19);
simontruelove 21:b831f68ce5ed 1170 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1171 }
simontruelove 20:dca9f4c12fe3 1172 else if(rpm > 56 and rpm < 76)
simontruelove 19:d9ba2f225f39 1173 {
simontruelove 19:d9ba2f225f39 1174 AdjCW = (55);
simontruelove 19:d9ba2f225f39 1175 AdjACW = (19);
simontruelove 21:b831f68ce5ed 1176 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1177 }
simontruelove 20:dca9f4c12fe3 1178 else if(rpm > 76 and rpm < 90)
simontruelove 19:d9ba2f225f39 1179 {
simontruelove 19:d9ba2f225f39 1180 AdjCW = (58);
simontruelove 19:d9ba2f225f39 1181 AdjACW = (19);
simontruelove 21:b831f68ce5ed 1182 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1183 }
simontruelove 20:dca9f4c12fe3 1184 else if(rpm > 90 and rpm < 125)
simontruelove 16:e6c8df9960c6 1185 {
simontruelove 19:d9ba2f225f39 1186 AdjCW = (58);
simontruelove 19:d9ba2f225f39 1187 AdjACW = (14);
simontruelove 21:b831f68ce5ed 1188 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1189 }
simontruelove 20:dca9f4c12fe3 1190 else if(rpm > 125 and rpm < 140)
simontruelove 19:d9ba2f225f39 1191 {
simontruelove 19:d9ba2f225f39 1192 AdjCW = (62);
simontruelove 17:19b2c598810a 1193 AdjACW = (14);
simontruelove 21:b831f68ce5ed 1194 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1195 }
simontruelove 20:dca9f4c12fe3 1196 else if(rpm > 140 and rpm < 190)
simontruelove 19:d9ba2f225f39 1197 {
simontruelove 19:d9ba2f225f39 1198 AdjCW = (62);
simontruelove 19:d9ba2f225f39 1199 AdjACW = (9);
simontruelove 21:b831f68ce5ed 1200 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1201 }
simontruelove 20:dca9f4c12fe3 1202 else if(rpm > 190 and rpm < 200)
simontruelove 16:e6c8df9960c6 1203 {
simontruelove 19:d9ba2f225f39 1204 AdjCW = (3);
simontruelove 19:d9ba2f225f39 1205 AdjACW = (9);
simontruelove 21:b831f68ce5ed 1206 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1207 }
simontruelove 20:dca9f4c12fe3 1208 else if(rpm > 200 and rpm < 350)
simontruelove 19:d9ba2f225f39 1209 {
simontruelove 19:d9ba2f225f39 1210 AdjCW = (3);
simontruelove 19:d9ba2f225f39 1211 AdjACW = (9);
simontruelove 21:b831f68ce5ed 1212 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1213 }
simontruelove 20:dca9f4c12fe3 1214 else if(rpm > 350 and rpm < 650)
simontruelove 19:d9ba2f225f39 1215 {
simontruelove 19:d9ba2f225f39 1216 AdjCW = (3);
simontruelove 19:d9ba2f225f39 1217 AdjACW = (5);
simontruelove 21:b831f68ce5ed 1218 gain = (0.0001);
simontruelove 19:d9ba2f225f39 1219 }
simontruelove 20:dca9f4c12fe3 1220 else if(rpm > 650)
simontruelove 19:d9ba2f225f39 1221 {
simontruelove 19:d9ba2f225f39 1222 AdjCW = (6);
simontruelove 16:e6c8df9960c6 1223 AdjACW = (5);
simontruelove 21:b831f68ce5ed 1224 gain = (0.000001);
simontruelove 16:e6c8df9960c6 1225 }
simontruelove 22:58558001804d 1226 }*/
simontruelove 7:b8de1529c7fc 1227 }
simontruelove 7:b8de1529c7fc 1228
simontruelove 22:58558001804d 1229 void VelocityLoop (void) //Speed control
simontruelove 7:b8de1529c7fc 1230 {
simontruelove 20:dca9f4c12fe3 1231 diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
simontruelove 20:dca9f4c12fe3 1232 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
simontruelove 20:dca9f4c12fe3 1233
simontruelove 20:dca9f4c12fe3 1234 if (duty > 1) //limits for duty. Motor will not operate below 0.11. 1 = max
simontruelove 14:1eb49362a607 1235 {
simontruelove 9:061600a6c750 1236 duty = 1;
simontruelove 7:b8de1529c7fc 1237 }
simontruelove 22:58558001804d 1238 if (duty <0.053) //min duty 0.11
simontruelove 10:808cb9052f14 1239 {
simontruelove 22:58558001804d 1240 duty = 0.053;
simontruelove 7:b8de1529c7fc 1241 }
simontruelove 11:74eeb8871fe6 1242 }
simontruelove 11:74eeb8871fe6 1243
simontruelove 20:dca9f4c12fe3 1244 void ReadKType(void) //Reads Temperature
simontruelove 11:74eeb8871fe6 1245 {
simontruelove 22:58558001804d 1246 int i = 0;
simontruelove 11:74eeb8871fe6 1247 int Readout = 0;
simontruelove 11:74eeb8871fe6 1248
simontruelove 11:74eeb8871fe6 1249 cs1 = 0;
simontruelove 20:dca9f4c12fe3 1250 SerialClock = 0; //set clock to 0
simontruelove 16:e6c8df9960c6 1251 wait_ms(0.01);
simontruelove 20:dca9f4c12fe3 1252 SerialClock = 1; //clock once to set to the 13 bit temp data
simontruelove 16:e6c8df9960c6 1253 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1254 SerialClock = 0;
simontruelove 16:e6c8df9960c6 1255 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1256
simontruelove 22:58558001804d 1257 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
simontruelove 11:74eeb8871fe6 1258 {
simontruelove 20:dca9f4c12fe3 1259 if(DOut == 1) //check data, store results in readout
simontruelove 11:74eeb8871fe6 1260 {
simontruelove 12:cbea987a3ec4 1261 Readout |= (1<<i);
simontruelove 11:74eeb8871fe6 1262 }
simontruelove 11:74eeb8871fe6 1263 else
simontruelove 11:74eeb8871fe6 1264 {
simontruelove 11:74eeb8871fe6 1265 Readout |= (0<<i);
simontruelove 11:74eeb8871fe6 1266 }
simontruelove 12:cbea987a3ec4 1267
simontruelove 20:dca9f4c12fe3 1268 SerialClock = 1; //clock to the next bit
simontruelove 16:e6c8df9960c6 1269 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1270 SerialClock = 0;
simontruelove 16:e6c8df9960c6 1271 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1272 }
simontruelove 20:dca9f4c12fe3 1273 temp = Readout * 0.125; //get the real temp value which is a float
simontruelove 11:74eeb8871fe6 1274 Readout = 0;
simontruelove 11:74eeb8871fe6 1275 cs1 = 1;
simontruelove 22:58558001804d 1276 }
simontruelove 22:58558001804d 1277
simontruelove 22:58558001804d 1278 void Switch (void)
simontruelove 22:58558001804d 1279 {
simontruelove 22:58558001804d 1280 led1 = clockwise;
simontruelove 22:58558001804d 1281 led2 = anticlockwise;
simontruelove 3:4249dbdf7ed3 1282 }