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@16:e6c8df9960c6, 2019-02-06 (annotated)

Committer:
simontruelove
Date:
Wed Feb 06 15:46:51 2019 +0000
Revision:
16:e6c8df9960c6
Parent:
15:12a4bbfa6de4
Child:
17:19b2c598810a
Optimised 3Ph0 state machine with basic phase mapping in both directions at load and detailed phase mapping in both directions at no load (duty < 0.7)

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 3:4249dbdf7ed3 3
simontruelove 10:808cb9052f14 4 void Initialisation (void); //These voids are written after the main. They must be listed here too (functional prototypes).
simontruelove 16:e6c8df9960c6 5 void StepACW(void);
simontruelove 13:da9d3fbbe407 6 void Ph0(void);
simontruelove 1:0191658b6ff4 7 void Ph1(void);
simontruelove 3:4249dbdf7ed3 8 void Ph12 (void);
simontruelove 1:0191658b6ff4 9 void Ph2(void);
simontruelove 3:4249dbdf7ed3 10 void Ph23 (void);
simontruelove 1:0191658b6ff4 11 void Ph3(void);
simontruelove 3:4249dbdf7ed3 12 void Ph34 (void);
simontruelove 1:0191658b6ff4 13 void Ph4(void);
simontruelove 3:4249dbdf7ed3 14 void Ph41 (void);
simontruelove 4:3aedc9246ae4 15 void GetChar (void);
simontruelove 5:4e5c644d5cc3 16 void RPM (void);
simontruelove 7:b8de1529c7fc 17 void VelocityLoop (void);
simontruelove 11:74eeb8871fe6 18 void ReadKType(void);
simontruelove 1:0191658b6ff4 19
simontruelove 10:808cb9052f14 20 Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
simontruelove 1:0191658b6ff4 21
simontruelove 10:808cb9052f14 22 QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
simontruelove 3:4249dbdf7ed3 23
simontruelove 10:808cb9052f14 24 Timer t; //timer used in RPM
simontruelove 3:4249dbdf7ed3 25
simontruelove 11:74eeb8871fe6 26 PwmOut Phase1 (p21); //Pin and LED set up - originally standard pins but changed to PWM to enable speed control
simontruelove 12:cbea987a3ec4 27 PwmOut Phase2 (p22);
simontruelove 16:e6c8df9960c6 28 PwmOut Phase3 (p23);
simontruelove 16:e6c8df9960c6 29 PwmOut Phase4 (p24);
simontruelove 16:e6c8df9960c6 30 DigitalOut UnUsedPhase1 (p25);
simontruelove 16:e6c8df9960c6 31 DigitalOut UnUsedPhase2 (p26);
simontruelove 1:0191658b6ff4 32
simontruelove 12:cbea987a3ec4 33 //AnalogOut Aout(p18); //Used with multimeter to give a speed indicator 1mV = 1RPM
simontruelove 6:f7028034aabb 34
simontruelove 10:808cb9052f14 35 //DigitalIn Button1 (p11); //not used
simontruelove 10:808cb9052f14 36 //DigitalIn Button2 (p12); //not used
simontruelove 2:3f95c82c26bb 37
simontruelove 10:808cb9052f14 38 DigitalOut led1(LED1); //LEDs used to as very basic memmory for controlling the state machines
simontruelove 2:3f95c82c26bb 39 DigitalOut led2(LED2);
simontruelove 2:3f95c82c26bb 40 DigitalOut led3(LED3);
simontruelove 2:3f95c82c26bb 41 DigitalOut led4(LED4);
simontruelove 10:808cb9052f14 42 DigitalOut SerialClock (p12); //ReadKType
simontruelove 10:808cb9052f14 43 DigitalIn DOut (p13); //ReadKType
simontruelove 10:808cb9052f14 44 DigitalOut cs1 (p14); //ReadKType
simontruelove 10:808cb9052f14 45
simontruelove 10:808cb9052f14 46 int StateA = 0; //State for first 2 revolutions (calibration of the index)
simontruelove 10:808cb9052f14 47 int StateB = 0; //All state machines after calibration use this state
simontruelove 6:f7028034aabb 48 //int StateC = 0;
simontruelove 14:1eb49362a607 49 int AdjCW = 57; //2 CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 14:1eb49362a607 50 int AdjACW = 12; //5 ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 14:1eb49362a607 51 int CW = 57;
simontruelove 14:1eb49362a607 52 int ACW = 12;
simontruelove 10:808cb9052f14 53 int TimePerClick = 0; //for calc of RPM
simontruelove 12:cbea987a3ec4 54 int RPS = 0; //for calc of RPM
simontruelove 10:808cb9052f14 55 int rpm = 0; //for calc of RPM
simontruelove 14:1eb49362a607 56 int SetPoint = 1500; //for adjusting the speed
simontruelove 14:1eb49362a607 57 int enc = 3200;
simontruelove 16:e6c8df9960c6 58 int i = 0;
simontruelove 16:e6c8df9960c6 59 int j = 0;
simontruelove 16:e6c8df9960c6 60 int k = 0;
simontruelove 16:e6c8df9960c6 61 int l = 0;
simontruelove 16:e6c8df9960c6 62 int m = 0;
simontruelove 16:e6c8df9960c6 63 int n = 0;
simontruelove 14:1eb49362a607 64 int s = enc/50;
simontruelove 14:1eb49362a607 65 int z = 3200; //TimePerRev = TimePerClick * (800/z); 800 pulses per rev, PulseCount2_==800 for wheel.getwhoop_ flag. i.e. 10 points per reoluition for RPM calc.
simontruelove 11:74eeb8871fe6 66 int T = 80; //Motor temp limit
simontruelove 14:1eb49362a607 67 int slowloop = 0;
simontruelove 11:74eeb8871fe6 68
simontruelove 10:808cb9052f14 69 char c; //keyboard cotrol GetChar
simontruelove 4:3aedc9246ae4 70
simontruelove 13:da9d3fbbe407 71 float duty = 1; //velocity loop: 1 = fastest, 0.96 = slowest. Below 0.96 the motor will not operate.
simontruelove 10:808cb9052f14 72 float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
simontruelove 16:e6c8df9960c6 73 float AdjDiff = 0.00001;
simontruelove 16:e6c8df9960c6 74 float p = 0.000014;
simontruelove 10:808cb9052f14 75 float x=0.1; //x=time of square wave when 1 phase energised,
simontruelove 14:1eb49362a607 76 float TimePerRev = 0; //for calc of RPM
simontruelove 10:808cb9052f14 77 float y=0.04; //y=time of square wave when 2 phases energised
simontruelove 10:808cb9052f14 78 float temp = 0; //ReadKType
simontruelove 1:0191658b6ff4 79
simontruelove 1:0191658b6ff4 80 int main(void)
simontruelove 3:4249dbdf7ed3 81 {
simontruelove 10:808cb9052f14 82 pc.baud(230400); //Set fastest baud rate
simontruelove 16:e6c8df9960c6 83 Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
simontruelove 16:e6c8df9960c6 84 Phase2.period(p);
simontruelove 16:e6c8df9960c6 85 Phase3.period(p);
simontruelove 16:e6c8df9960c6 86 Phase4.period(p);
simontruelove 14:1eb49362a607 87 wait(0.1);
simontruelove 14:1eb49362a607 88 t.start();
simontruelove 14:1eb49362a607 89 SerialClock = 0;
simontruelove 16:e6c8df9960c6 90 StepACW();
simontruelove 3:4249dbdf7ed3 91 Initialisation();
simontruelove 14:1eb49362a607 92
simontruelove 14:1eb49362a607 93 /*while(1)
simontruelove 14:1eb49362a607 94 {
simontruelove 14:1eb49362a607 95
simontruelove 14:1eb49362a607 96 }
simontruelove 14:1eb49362a607 97 */
simontruelove 14:1eb49362a607 98
simontruelove 10:808cb9052f14 99 while(wheel.getRevolutions()<2) //Index Calibration
simontruelove 14:1eb49362a607 100 {
simontruelove 14:1eb49362a607 101 StateA = (wheel.getPulses()+25)%s;
simontruelove 5:4e5c644d5cc3 102 switch(StateA)
simontruelove 3:4249dbdf7ed3 103 {
simontruelove 14:1eb49362a607 104 case 0:Ph1();break;//;pc.printf("1 Pulses= %i\n\r", wheel.getPulses());break;
simontruelove 6:f7028034aabb 105 case 1:Ph1();break;
simontruelove 14:1eb49362a607 106 case 2:Ph1();break;
simontruelove 14:1eb49362a607 107 case 3:Ph1();break;
simontruelove 14:1eb49362a607 108 case 4:Ph1();break;
simontruelove 14:1eb49362a607 109 case 5:Ph1();break;
simontruelove 14:1eb49362a607 110 case 6:Ph1();break;
simontruelove 14:1eb49362a607 111 case 7:Ph1();break;
simontruelove 14:1eb49362a607 112 case 8:Ph1();break;
simontruelove 14:1eb49362a607 113 case 9:Ph1();break;
simontruelove 14:1eb49362a607 114 case 10:Ph1();break;
simontruelove 14:1eb49362a607 115 case 11:Ph1();break;
simontruelove 14:1eb49362a607 116 case 12:Ph1();break;
simontruelove 14:1eb49362a607 117 case 13:Ph1();break;
simontruelove 14:1eb49362a607 118 case 14:Ph1();break;
simontruelove 14:1eb49362a607 119 case 15:Ph1();break;
simontruelove 14:1eb49362a607 120 case 16:Ph2();break;//;pc.printf("2 Pulses= %i\n\r", wheel.getPulses());break;
simontruelove 14:1eb49362a607 121 case 17:Ph2();break;
simontruelove 14:1eb49362a607 122 case 18:Ph2();break;
simontruelove 14:1eb49362a607 123 case 19:Ph2();break;
simontruelove 14:1eb49362a607 124 case 20:Ph2();break;
simontruelove 14:1eb49362a607 125 case 21:Ph2();break;
simontruelove 14:1eb49362a607 126 case 22:Ph2();break;
simontruelove 14:1eb49362a607 127 case 23:Ph2();break;
simontruelove 14:1eb49362a607 128 case 24:Ph2();break;
simontruelove 14:1eb49362a607 129 case 25:Ph2();break;
simontruelove 14:1eb49362a607 130 case 26:Ph2();break;
simontruelove 14:1eb49362a607 131 case 27:Ph2();break;
simontruelove 14:1eb49362a607 132 case 28:Ph2();break;
simontruelove 14:1eb49362a607 133 case 29:Ph2();break;
simontruelove 14:1eb49362a607 134 case 30:Ph2();break;
simontruelove 14:1eb49362a607 135 case 31:Ph2();break;
simontruelove 14:1eb49362a607 136 case 32:Ph3();break;//;pc.printf("3 Pulses= %i\n\r", wheel.getPulses());break;
simontruelove 14:1eb49362a607 137 case 33:Ph3();break;
simontruelove 14:1eb49362a607 138 case 34:Ph3();break;
simontruelove 14:1eb49362a607 139 case 35:Ph3();break;
simontruelove 14:1eb49362a607 140 case 36:Ph3();break;
simontruelove 14:1eb49362a607 141 case 37:Ph3();break;
simontruelove 14:1eb49362a607 142 case 38:Ph3();break;
simontruelove 14:1eb49362a607 143 case 39:Ph3();break;
simontruelove 14:1eb49362a607 144 case 40:Ph3();break;
simontruelove 14:1eb49362a607 145 case 41:Ph3();break;
simontruelove 14:1eb49362a607 146 case 42:Ph3();break;
simontruelove 14:1eb49362a607 147 case 43:Ph3();break;
simontruelove 14:1eb49362a607 148 case 44:Ph3();break;
simontruelove 14:1eb49362a607 149 case 45:Ph3();break;
simontruelove 14:1eb49362a607 150 case 46:Ph3();break;
simontruelove 14:1eb49362a607 151 case 47:Ph3();break;
simontruelove 14:1eb49362a607 152 case 48:Ph4();break;//;pc.printf("4 Pulses= %i\n\r", wheel.getPulses());break;
simontruelove 14:1eb49362a607 153 case 49:Ph4();break;
simontruelove 14:1eb49362a607 154 case 50:Ph4();break;
simontruelove 14:1eb49362a607 155 case 51:Ph4();break;
simontruelove 14:1eb49362a607 156 case 52:Ph4();break;
simontruelove 14:1eb49362a607 157 case 53:Ph4();break;
simontruelove 14:1eb49362a607 158 case 54:Ph4();break;
simontruelove 14:1eb49362a607 159 case 55:Ph4();break;
simontruelove 14:1eb49362a607 160 case 56:Ph4();break;
simontruelove 14:1eb49362a607 161 case 57:Ph4();break;
simontruelove 14:1eb49362a607 162 case 58:Ph4();break;
simontruelove 14:1eb49362a607 163 case 59:Ph4();break;
simontruelove 14:1eb49362a607 164 case 60:Ph4();break;
simontruelove 14:1eb49362a607 165 case 61:Ph4();break;
simontruelove 14:1eb49362a607 166 case 62:Ph4();break;
simontruelove 14:1eb49362a607 167 case 63:Ph4();break;
simontruelove 5:4e5c644d5cc3 168 default:break;
simontruelove 3:4249dbdf7ed3 169 }
simontruelove 10:808cb9052f14 170 if(wheel.getYay()==1) //PulseCount_==1, yay_=1;
simontruelove 3:4249dbdf7ed3 171 {
simontruelove 5:4e5c644d5cc3 172 StateA++;
simontruelove 5:4e5c644d5cc3 173 wheel.ResetYay();
simontruelove 14:1eb49362a607 174 if (StateA>(s-1))
simontruelove 3:4249dbdf7ed3 175 {
simontruelove 5:4e5c644d5cc3 176 StateA=0;
simontruelove 3:4249dbdf7ed3 177 }
simontruelove 3:4249dbdf7ed3 178 }
simontruelove 14:1eb49362a607 179 }
simontruelove 3:4249dbdf7ed3 180
simontruelove 5:4e5c644d5cc3 181 while(1)
simontruelove 5:4e5c644d5cc3 182 {
simontruelove 10:808cb9052f14 183 while((led1 == 0) && (led2 == 0)) //If no command to operate
simontruelove 4:3aedc9246ae4 184 {
simontruelove 12:cbea987a3ec4 185 //Aout = 0;
simontruelove 16:e6c8df9960c6 186 //duty = 0.7;
simontruelove 10:808cb9052f14 187 rpm = 0;
simontruelove 14:1eb49362a607 188 AdjCW = CW;
simontruelove 14:1eb49362a607 189 AdjACW = ACW;
simontruelove 14:1eb49362a607 190 Ph0();
simontruelove 10:808cb9052f14 191 GetChar();
simontruelove 11:74eeb8871fe6 192 ReadKType();
simontruelove 16:e6c8df9960c6 193 //VelocityLoop();
simontruelove 14:1eb49362a607 194 StateB = (wheel.getPulses()+StateA)%s; //wheel.getPulses()%1(s-1);
simontruelove 16:e6c8df9960c6 195 //wait(0.1);
simontruelove 14:1eb49362a607 196 //StateC = (800+wheel.getPulses()+StateA+AdjCW)%s;
simontruelove 14:1eb49362a607 197 //if(wheel.getPulses()==wheel.getPulses()+1);
simontruelove 14:1eb49362a607 198 //{
simontruelove 14:1eb49362a607 199 //pc.printf("B StateA= %i, Pulses= %i, Revs= %i\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 200 //pc.printf("StateA= %i, StateB= %i, Pulses = %i \r", StateA, StateB, wheel.getPulses());
simontruelove 14:1eb49362a607 201 //}
simontruelove 5:4e5c644d5cc3 202 //pc.printf("0 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 203 }
simontruelove 11:74eeb8871fe6 204 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
simontruelove 3:4249dbdf7ed3 205 {
simontruelove 4:3aedc9246ae4 206 GetChar();
simontruelove 16:e6c8df9960c6 207 //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
simontruelove 16:e6c8df9960c6 208 //Phase2.period(p);
simontruelove 16:e6c8df9960c6 209 //Phase3.period(p);
simontruelove 16:e6c8df9960c6 210 //Phase4.period(p);
simontruelove 14:1eb49362a607 211 StateB = (wheel.getPulses()+StateA+AdjCW)%s;
simontruelove 14:1eb49362a607 212 //pc.printf("rpm = %i, whoop = %i\n\r", rpm, wheel.getWhoop());
simontruelove 6:f7028034aabb 213 //pc.printf("StateB= %i\n\r", StateB);
simontruelove 14:1eb49362a607 214 //pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 215
simontruelove 4:3aedc9246ae4 216 switch(StateB)
simontruelove 4:3aedc9246ae4 217 {
simontruelove 6:f7028034aabb 218 case 0:Ph1();break;
simontruelove 6:f7028034aabb 219 case 1:Ph1();break;
simontruelove 13:da9d3fbbe407 220 case 2:Ph1();break;
simontruelove 13:da9d3fbbe407 221 case 3:Ph1();break;
simontruelove 14:1eb49362a607 222 case 4:Ph1();break;
simontruelove 14:1eb49362a607 223 case 5:Ph1();break;
simontruelove 14:1eb49362a607 224 case 6:Ph1();break;
simontruelove 14:1eb49362a607 225 case 7:Ph1();break;
simontruelove 14:1eb49362a607 226 case 8:Ph1();break;
simontruelove 14:1eb49362a607 227 case 9:Ph1();break;
simontruelove 16:e6c8df9960c6 228 case 10:Ph0();break;
simontruelove 14:1eb49362a607 229 case 11:Ph1();break;
simontruelove 14:1eb49362a607 230 case 12:Ph1();break;
simontruelove 16:e6c8df9960c6 231 case 13:Ph0();break;
simontruelove 14:1eb49362a607 232 case 14:Ph1();break;
simontruelove 16:e6c8df9960c6 233 case 15:Ph0();break;
simontruelove 14:1eb49362a607 234 case 16:Ph2();break;
simontruelove 14:1eb49362a607 235 case 17:Ph2();break;
simontruelove 14:1eb49362a607 236 case 18:Ph2();break;
simontruelove 14:1eb49362a607 237 case 19:Ph2();break;
simontruelove 14:1eb49362a607 238 case 20:Ph2();break;
simontruelove 14:1eb49362a607 239 case 21:Ph2();break;
simontruelove 14:1eb49362a607 240 case 22:Ph2();break;
simontruelove 14:1eb49362a607 241 case 23:Ph2();break;
simontruelove 14:1eb49362a607 242 case 24:Ph2();break;
simontruelove 14:1eb49362a607 243 case 25:Ph2();break;
simontruelove 16:e6c8df9960c6 244 case 26:Ph0();break;
simontruelove 14:1eb49362a607 245 case 27:Ph2();break;
simontruelove 14:1eb49362a607 246 case 28:Ph2();break;
simontruelove 16:e6c8df9960c6 247 case 29:Ph0();break;
simontruelove 14:1eb49362a607 248 case 30:Ph2();break;
simontruelove 16:e6c8df9960c6 249 case 31:Ph0();break;
simontruelove 14:1eb49362a607 250 case 32:Ph3();break;
simontruelove 14:1eb49362a607 251 case 33:Ph3();break;
simontruelove 14:1eb49362a607 252 case 34:Ph3();break;
simontruelove 14:1eb49362a607 253 case 35:Ph3();break;
simontruelove 14:1eb49362a607 254 case 36:Ph3();break;
simontruelove 14:1eb49362a607 255 case 37:Ph3();break;
simontruelove 14:1eb49362a607 256 case 38:Ph3();break;
simontruelove 14:1eb49362a607 257 case 39:Ph3();break;
simontruelove 14:1eb49362a607 258 case 40:Ph3();break;
simontruelove 14:1eb49362a607 259 case 41:Ph3();break;
simontruelove 16:e6c8df9960c6 260 case 42:Ph0();break;
simontruelove 14:1eb49362a607 261 case 43:Ph3();break;
simontruelove 14:1eb49362a607 262 case 44:Ph3();break;
simontruelove 16:e6c8df9960c6 263 case 45:Ph0();break;
simontruelove 14:1eb49362a607 264 case 46:Ph3();break;
simontruelove 16:e6c8df9960c6 265 case 47:Ph0();break;
simontruelove 14:1eb49362a607 266 case 48:Ph4();break;
simontruelove 14:1eb49362a607 267 case 49:Ph4();break;
simontruelove 14:1eb49362a607 268 case 50:Ph4();break;
simontruelove 14:1eb49362a607 269 case 51:Ph4();break;
simontruelove 14:1eb49362a607 270 case 52:Ph4();break;
simontruelove 14:1eb49362a607 271 case 53:Ph4();break;
simontruelove 14:1eb49362a607 272 case 54:Ph4();break;
simontruelove 14:1eb49362a607 273 case 55:Ph4();break;
simontruelove 14:1eb49362a607 274 case 56:Ph4();break;
simontruelove 14:1eb49362a607 275 case 57:Ph4();break;
simontruelove 16:e6c8df9960c6 276 case 58:Ph0();break;
simontruelove 14:1eb49362a607 277 case 59:Ph4();break;
simontruelove 14:1eb49362a607 278 case 60:Ph4();break;
simontruelove 16:e6c8df9960c6 279 case 61:Ph0();break;
simontruelove 14:1eb49362a607 280 case 62:Ph4();break;
simontruelove 16:e6c8df9960c6 281 case 63:Ph0();break;
simontruelove 16:e6c8df9960c6 282 default:break;
simontruelove 4:3aedc9246ae4 283 }
simontruelove 14:1eb49362a607 284 /*if(wheel.getYay()==1)
simontruelove 14:1eb49362a607 285 {
simontruelove 14:1eb49362a607 286 pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 287 }*/
simontruelove 16:e6c8df9960c6 288 if(wheel.getWhoop()==1) //PulseCount2_==3200, whoop_=1;
simontruelove 4:3aedc9246ae4 289 {
simontruelove 5:4e5c644d5cc3 290 RPM();
simontruelove 9:061600a6c750 291 VelocityLoop();
simontruelove 14:1eb49362a607 292 slowloop++;
simontruelove 16:e6c8df9960c6 293 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 294 {
simontruelove 14:1eb49362a607 295 ReadKType();
simontruelove 14:1eb49362a607 296 slowloop=0;
simontruelove 16:e6c8df9960c6 297 //pc.printf("%i, %.4f\n\r", rpm, duty);
simontruelove 14:1eb49362a607 298 }
simontruelove 11:74eeb8871fe6 299 }
simontruelove 4:3aedc9246ae4 300 }
simontruelove 4:3aedc9246ae4 301
simontruelove 11:74eeb8871fe6 302 while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
simontruelove 4:3aedc9246ae4 303 {
simontruelove 4:3aedc9246ae4 304 GetChar();
simontruelove 16:e6c8df9960c6 305 //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
simontruelove 16:e6c8df9960c6 306 //Phase2.period(p);
simontruelove 16:e6c8df9960c6 307 //Phase3.period(p);
simontruelove 16:e6c8df9960c6 308 //Phase4.period(p);
simontruelove 14:1eb49362a607 309 StateB = (enc+wheel.getPulses()+StateA+AdjCW)%s;
simontruelove 6:f7028034aabb 310 //pc.printf("StateA= %i\r", StateA);
simontruelove 14:1eb49362a607 311 //pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 3:4249dbdf7ed3 312
simontruelove 3:4249dbdf7ed3 313 switch(StateB)
simontruelove 3:4249dbdf7ed3 314 {
simontruelove 6:f7028034aabb 315 case 0:Ph1();break;
simontruelove 6:f7028034aabb 316 case 1:Ph1();break;
simontruelove 13:da9d3fbbe407 317 case 2:Ph1();break;
simontruelove 13:da9d3fbbe407 318 case 3:Ph1();break;
simontruelove 14:1eb49362a607 319 case 4:Ph1();break;
simontruelove 14:1eb49362a607 320 case 5:Ph1();break;
simontruelove 14:1eb49362a607 321 case 6:Ph1();break;
simontruelove 14:1eb49362a607 322 case 7:Ph1();break;
simontruelove 14:1eb49362a607 323 case 8:Ph1();break;
simontruelove 14:1eb49362a607 324 case 9:Ph1();break;
simontruelove 16:e6c8df9960c6 325 case 10:Ph0();break;
simontruelove 14:1eb49362a607 326 case 11:Ph1();break;
simontruelove 14:1eb49362a607 327 case 12:Ph1();break;
simontruelove 16:e6c8df9960c6 328 case 13:Ph0();break;
simontruelove 14:1eb49362a607 329 case 14:Ph1();break;
simontruelove 16:e6c8df9960c6 330 case 15:Ph0();break;
simontruelove 14:1eb49362a607 331 case 16:Ph2();break;
simontruelove 14:1eb49362a607 332 case 17:Ph2();break;
simontruelove 14:1eb49362a607 333 case 18:Ph2();break;
simontruelove 14:1eb49362a607 334 case 19:Ph2();break;
simontruelove 14:1eb49362a607 335 case 20:Ph2();break;
simontruelove 14:1eb49362a607 336 case 21:Ph2();break;
simontruelove 14:1eb49362a607 337 case 22:Ph2();break;
simontruelove 14:1eb49362a607 338 case 23:Ph2();break;
simontruelove 14:1eb49362a607 339 case 24:Ph2();break;
simontruelove 14:1eb49362a607 340 case 25:Ph2();break;
simontruelove 16:e6c8df9960c6 341 case 26:Ph0();break;
simontruelove 14:1eb49362a607 342 case 27:Ph2();break;
simontruelove 14:1eb49362a607 343 case 28:Ph2();break;
simontruelove 16:e6c8df9960c6 344 case 29:Ph0();break;
simontruelove 14:1eb49362a607 345 case 30:Ph2();break;
simontruelove 16:e6c8df9960c6 346 case 31:Ph0();break;
simontruelove 14:1eb49362a607 347 case 32:Ph3();break;
simontruelove 14:1eb49362a607 348 case 33:Ph3();break;
simontruelove 14:1eb49362a607 349 case 34:Ph3();break;
simontruelove 14:1eb49362a607 350 case 35:Ph3();break;
simontruelove 14:1eb49362a607 351 case 36:Ph3();break;
simontruelove 14:1eb49362a607 352 case 37:Ph3();break;
simontruelove 14:1eb49362a607 353 case 38:Ph3();break;
simontruelove 14:1eb49362a607 354 case 39:Ph3();break;
simontruelove 14:1eb49362a607 355 case 40:Ph3();break;
simontruelove 14:1eb49362a607 356 case 41:Ph3();break;
simontruelove 16:e6c8df9960c6 357 case 42:Ph0();break;
simontruelove 14:1eb49362a607 358 case 43:Ph3();break;
simontruelove 14:1eb49362a607 359 case 44:Ph3();break;
simontruelove 16:e6c8df9960c6 360 case 45:Ph0();break;
simontruelove 14:1eb49362a607 361 case 46:Ph3();break;
simontruelove 16:e6c8df9960c6 362 case 47:Ph0();break;
simontruelove 14:1eb49362a607 363 case 48:Ph4();break;
simontruelove 14:1eb49362a607 364 case 49:Ph4();break;
simontruelove 14:1eb49362a607 365 case 50:Ph4();break;
simontruelove 14:1eb49362a607 366 case 51:Ph4();break;
simontruelove 14:1eb49362a607 367 case 52:Ph4();break;
simontruelove 14:1eb49362a607 368 case 53:Ph4();break;
simontruelove 14:1eb49362a607 369 case 54:Ph4();break;
simontruelove 14:1eb49362a607 370 case 55:Ph4();break;
simontruelove 14:1eb49362a607 371 case 56:Ph4();break;
simontruelove 14:1eb49362a607 372 case 57:Ph4();break;
simontruelove 16:e6c8df9960c6 373 case 58:Ph0();break;
simontruelove 14:1eb49362a607 374 case 59:Ph4();break;
simontruelove 14:1eb49362a607 375 case 60:Ph4();break;
simontruelove 16:e6c8df9960c6 376 case 61:Ph0();break;
simontruelove 14:1eb49362a607 377 case 62:Ph4();break;
simontruelove 16:e6c8df9960c6 378 case 63:Ph0();break;
simontruelove 5:4e5c644d5cc3 379 default:break;
simontruelove 3:4249dbdf7ed3 380 }
simontruelove 14:1eb49362a607 381 /*if(wheel.getYay()==1)
simontruelove 14:1eb49362a607 382 {
simontruelove 14:1eb49362a607 383 pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 384 }*/
simontruelove 16:e6c8df9960c6 385 if(wheel.getWhoop()==1) //PulseCount2_==3200, whoop_=1;
simontruelove 5:4e5c644d5cc3 386 {
simontruelove 5:4e5c644d5cc3 387 RPM();
simontruelove 9:061600a6c750 388 VelocityLoop();
simontruelove 14:1eb49362a607 389 slowloop++;
simontruelove 16:e6c8df9960c6 390 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 391 {
simontruelove 14:1eb49362a607 392 ReadKType();
simontruelove 14:1eb49362a607 393 slowloop=0;
simontruelove 16:e6c8df9960c6 394 //pc.printf("%i, %.4f\n\r", rpm, duty);
simontruelove 14:1eb49362a607 395 }
simontruelove 11:74eeb8871fe6 396 }
simontruelove 5:4e5c644d5cc3 397 }
simontruelove 11:74eeb8871fe6 398 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
simontruelove 4:3aedc9246ae4 399 {
simontruelove 5:4e5c644d5cc3 400 GetChar();
simontruelove 16:e6c8df9960c6 401 //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
simontruelove 16:e6c8df9960c6 402 //Phase2.period(p);
simontruelove 16:e6c8df9960c6 403 //Phase3.period(p);
simontruelove 16:e6c8df9960c6 404 //Phase4.period(p);
simontruelove 6:f7028034aabb 405 //StateB = (800+wheel.getPulses())%16;
simontruelove 14:1eb49362a607 406 StateB = (wheel.getPulses()+StateA+AdjACW)%s;
simontruelove 6:f7028034aabb 407 //pc.printf("StateA= %i\r", StateA);
simontruelove 14:1eb49362a607 408 //pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 409 switch(StateB)
simontruelove 5:4e5c644d5cc3 410 {
simontruelove 14:1eb49362a607 411 case 63:Ph4();break;
simontruelove 14:1eb49362a607 412 case 62:Ph4();break;
simontruelove 14:1eb49362a607 413 case 61:Ph4();break;
simontruelove 14:1eb49362a607 414 case 60:Ph4();break;
simontruelove 14:1eb49362a607 415 case 59:Ph4();break;
simontruelove 14:1eb49362a607 416 case 58:Ph4();break;
simontruelove 14:1eb49362a607 417 case 57:Ph4();break;
simontruelove 14:1eb49362a607 418 case 56:Ph4();break;
simontruelove 14:1eb49362a607 419 case 55:Ph4();break;
simontruelove 14:1eb49362a607 420 case 54:Ph4();break;
simontruelove 16:e6c8df9960c6 421 case 53:Ph0();break;
simontruelove 14:1eb49362a607 422 case 52:Ph4();break;
simontruelove 14:1eb49362a607 423 case 51:Ph4();break;
simontruelove 16:e6c8df9960c6 424 case 50:Ph0();break;
simontruelove 14:1eb49362a607 425 case 49:Ph4();break;
simontruelove 16:e6c8df9960c6 426 case 48:Ph0();break;
simontruelove 14:1eb49362a607 427 case 47:Ph3();break;
simontruelove 14:1eb49362a607 428 case 46:Ph3();break;
simontruelove 14:1eb49362a607 429 case 45:Ph3();break;
simontruelove 14:1eb49362a607 430 case 44:Ph3();break;
simontruelove 14:1eb49362a607 431 case 43:Ph3();break;
simontruelove 14:1eb49362a607 432 case 42:Ph3();break;
simontruelove 14:1eb49362a607 433 case 41:Ph3();break;
simontruelove 14:1eb49362a607 434 case 40:Ph3();break;
simontruelove 14:1eb49362a607 435 case 39:Ph3();break;
simontruelove 14:1eb49362a607 436 case 38:Ph3();break;
simontruelove 16:e6c8df9960c6 437 case 37:Ph0();break;
simontruelove 14:1eb49362a607 438 case 36:Ph3();break;
simontruelove 14:1eb49362a607 439 case 35:Ph3();break;
simontruelove 16:e6c8df9960c6 440 case 34:Ph0();break;
simontruelove 14:1eb49362a607 441 case 33:Ph3();break;
simontruelove 16:e6c8df9960c6 442 case 32:Ph0();break;
simontruelove 14:1eb49362a607 443 case 31:Ph2();break;
simontruelove 14:1eb49362a607 444 case 30:Ph2();break;
simontruelove 14:1eb49362a607 445 case 29:Ph2();break;
simontruelove 14:1eb49362a607 446 case 28:Ph2();break;
simontruelove 14:1eb49362a607 447 case 27:Ph2();break;
simontruelove 14:1eb49362a607 448 case 26:Ph2();break;
simontruelove 14:1eb49362a607 449 case 25:Ph2();break;
simontruelove 14:1eb49362a607 450 case 24:Ph2();break;
simontruelove 14:1eb49362a607 451 case 23:Ph2();break;
simontruelove 14:1eb49362a607 452 case 22:Ph2();break;
simontruelove 16:e6c8df9960c6 453 case 21:Ph0();break;
simontruelove 14:1eb49362a607 454 case 20:Ph2();break;
simontruelove 14:1eb49362a607 455 case 19:Ph2();break;
simontruelove 16:e6c8df9960c6 456 case 18:Ph0();break;
simontruelove 14:1eb49362a607 457 case 17:Ph2();break;
simontruelove 16:e6c8df9960c6 458 case 16:Ph0();break;
simontruelove 14:1eb49362a607 459 case 15:Ph1();break;
simontruelove 14:1eb49362a607 460 case 14:Ph1();break;
simontruelove 14:1eb49362a607 461 case 13:Ph1();break;
simontruelove 14:1eb49362a607 462 case 12:Ph1();break;
simontruelove 14:1eb49362a607 463 case 11:Ph1();break;
simontruelove 14:1eb49362a607 464 case 10:Ph1();break;
simontruelove 14:1eb49362a607 465 case 9:Ph1();break;
simontruelove 14:1eb49362a607 466 case 8:Ph1();break;
simontruelove 14:1eb49362a607 467 case 7:Ph1();break;
simontruelove 14:1eb49362a607 468 case 6:Ph1();break;
simontruelove 16:e6c8df9960c6 469 case 5:Ph0();break;
simontruelove 14:1eb49362a607 470 case 4:Ph1();break;
simontruelove 13:da9d3fbbe407 471 case 3:Ph1();break;
simontruelove 16:e6c8df9960c6 472 case 2:Ph0();break;
simontruelove 6:f7028034aabb 473 case 1:Ph1();break;
simontruelove 16:e6c8df9960c6 474 case 0:Ph0();break;
simontruelove 5:4e5c644d5cc3 475 default:break;
simontruelove 5:4e5c644d5cc3 476 }
simontruelove 14:1eb49362a607 477 /*if(wheel.getYay()==1)
simontruelove 14:1eb49362a607 478 {
simontruelove 14:1eb49362a607 479 pc.printf("3 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 480 }*/
simontruelove 16:e6c8df9960c6 481 if(wheel.getWhoop()==1) //PulseCount2_==3200, whoop_=1;
simontruelove 5:4e5c644d5cc3 482 {
simontruelove 5:4e5c644d5cc3 483 RPM();
simontruelove 9:061600a6c750 484 VelocityLoop();
simontruelove 14:1eb49362a607 485 slowloop++;
simontruelove 16:e6c8df9960c6 486 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 487 {
simontruelove 14:1eb49362a607 488 ReadKType();
simontruelove 14:1eb49362a607 489 slowloop=0;
simontruelove 16:e6c8df9960c6 490 //pc.printf("%i, %.4f\n\r", rpm, duty);
simontruelove 14:1eb49362a607 491 }
simontruelove 5:4e5c644d5cc3 492 }
simontruelove 3:4249dbdf7ed3 493 }
simontruelove 11:74eeb8871fe6 494 while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
simontruelove 5:4e5c644d5cc3 495 {
simontruelove 5:4e5c644d5cc3 496 GetChar();
simontruelove 16:e6c8df9960c6 497 //Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
simontruelove 16:e6c8df9960c6 498 //Phase2.period(p);
simontruelove 16:e6c8df9960c6 499 //Phase3.period(p);
simontruelove 16:e6c8df9960c6 500 //Phase4.period(p);
simontruelove 14:1eb49362a607 501 StateB = (enc+wheel.getPulses()+StateA+AdjACW)%s;
simontruelove 6:f7028034aabb 502 //pc.printf("StateA= %i\r", StateA);
simontruelove 14:1eb49362a607 503 //pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 504 switch(StateB)
simontruelove 5:4e5c644d5cc3 505 {
simontruelove 14:1eb49362a607 506 case 63:Ph4();break;
simontruelove 14:1eb49362a607 507 case 62:Ph4();break;
simontruelove 14:1eb49362a607 508 case 61:Ph4();break;
simontruelove 14:1eb49362a607 509 case 60:Ph4();break;
simontruelove 14:1eb49362a607 510 case 59:Ph4();break;
simontruelove 14:1eb49362a607 511 case 58:Ph4();break;
simontruelove 14:1eb49362a607 512 case 57:Ph4();break;
simontruelove 14:1eb49362a607 513 case 56:Ph4();break;
simontruelove 14:1eb49362a607 514 case 55:Ph4();break;
simontruelove 14:1eb49362a607 515 case 54:Ph4();break;
simontruelove 16:e6c8df9960c6 516 case 53:Ph0();break;
simontruelove 14:1eb49362a607 517 case 52:Ph4();break;
simontruelove 14:1eb49362a607 518 case 51:Ph4();break;
simontruelove 16:e6c8df9960c6 519 case 50:Ph0();break;
simontruelove 14:1eb49362a607 520 case 49:Ph4();break;
simontruelove 16:e6c8df9960c6 521 case 48:Ph0();break;
simontruelove 14:1eb49362a607 522 case 47:Ph3();break;
simontruelove 14:1eb49362a607 523 case 46:Ph3();break;
simontruelove 14:1eb49362a607 524 case 45:Ph3();break;
simontruelove 14:1eb49362a607 525 case 44:Ph3();break;
simontruelove 14:1eb49362a607 526 case 43:Ph3();break;
simontruelove 14:1eb49362a607 527 case 42:Ph3();break;
simontruelove 14:1eb49362a607 528 case 41:Ph3();break;
simontruelove 14:1eb49362a607 529 case 40:Ph3();break;
simontruelove 14:1eb49362a607 530 case 39:Ph3();break;
simontruelove 14:1eb49362a607 531 case 38:Ph3();break;
simontruelove 16:e6c8df9960c6 532 case 37:Ph0();break;
simontruelove 14:1eb49362a607 533 case 36:Ph3();break;
simontruelove 14:1eb49362a607 534 case 35:Ph3();break;
simontruelove 16:e6c8df9960c6 535 case 34:Ph0();break;
simontruelove 14:1eb49362a607 536 case 33:Ph3();break;
simontruelove 16:e6c8df9960c6 537 case 32:Ph0();break;
simontruelove 14:1eb49362a607 538 case 31:Ph2();break;
simontruelove 14:1eb49362a607 539 case 30:Ph2();break;
simontruelove 14:1eb49362a607 540 case 29:Ph2();break;
simontruelove 14:1eb49362a607 541 case 28:Ph2();break;
simontruelove 14:1eb49362a607 542 case 27:Ph2();break;
simontruelove 14:1eb49362a607 543 case 26:Ph2();break;
simontruelove 14:1eb49362a607 544 case 25:Ph2();break;
simontruelove 14:1eb49362a607 545 case 24:Ph2();break;
simontruelove 14:1eb49362a607 546 case 23:Ph2();break;
simontruelove 14:1eb49362a607 547 case 22:Ph2();break;
simontruelove 16:e6c8df9960c6 548 case 21:Ph0();break;
simontruelove 14:1eb49362a607 549 case 20:Ph2();break;
simontruelove 14:1eb49362a607 550 case 19:Ph2();break;
simontruelove 16:e6c8df9960c6 551 case 18:Ph0();break;
simontruelove 14:1eb49362a607 552 case 17:Ph2();break;
simontruelove 16:e6c8df9960c6 553 case 16:Ph0();break;
simontruelove 14:1eb49362a607 554 case 15:Ph1();break;
simontruelove 14:1eb49362a607 555 case 14:Ph1();break;
simontruelove 14:1eb49362a607 556 case 13:Ph1();break;
simontruelove 14:1eb49362a607 557 case 12:Ph1();break;
simontruelove 14:1eb49362a607 558 case 11:Ph1();break;
simontruelove 14:1eb49362a607 559 case 10:Ph1();break;
simontruelove 14:1eb49362a607 560 case 9:Ph1();break;
simontruelove 14:1eb49362a607 561 case 8:Ph1();break;
simontruelove 14:1eb49362a607 562 case 7:Ph1();break;
simontruelove 14:1eb49362a607 563 case 6:Ph1();break;
simontruelove 16:e6c8df9960c6 564 case 5:Ph0();break;
simontruelove 14:1eb49362a607 565 case 4:Ph1();break;
simontruelove 13:da9d3fbbe407 566 case 3:Ph1();break;
simontruelove 16:e6c8df9960c6 567 case 2:Ph0();break;
simontruelove 6:f7028034aabb 568 case 1:Ph1();break;
simontruelove 16:e6c8df9960c6 569 case 0:Ph0();break;
simontruelove 14:1eb49362a607 570 default:break;
simontruelove 5:4e5c644d5cc3 571 }
simontruelove 14:1eb49362a607 572 /*if(wheel.getYay()==1)
simontruelove 14:1eb49362a607 573 {
simontruelove 14:1eb49362a607 574 pc.printf("4 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 575 }*/
simontruelove 16:e6c8df9960c6 576 if(wheel.getWhoop()==1) //PulseCount2_==3200, whoop_=1;
simontruelove 5:4e5c644d5cc3 577 {
simontruelove 5:4e5c644d5cc3 578 RPM();
simontruelove 9:061600a6c750 579 VelocityLoop();
simontruelove 14:1eb49362a607 580 slowloop++;
simontruelove 16:e6c8df9960c6 581 if(slowloop>(0.01*rpm))
simontruelove 14:1eb49362a607 582 {
simontruelove 14:1eb49362a607 583 ReadKType();
simontruelove 14:1eb49362a607 584 slowloop=0;
simontruelove 16:e6c8df9960c6 585 //pc.printf("%i, %.4f\n\r", rpm, duty);
simontruelove 14:1eb49362a607 586 }
simontruelove 11:74eeb8871fe6 587 }
simontruelove 5:4e5c644d5cc3 588 }
simontruelove 11:74eeb8871fe6 589 while(temp>(T-1))
simontruelove 11:74eeb8871fe6 590 {
simontruelove 11:74eeb8871fe6 591 Initialisation();
simontruelove 11:74eeb8871fe6 592 pc.printf("Motor Over Temp\n\r");
simontruelove 11:74eeb8871fe6 593 while(1)
simontruelove 11:74eeb8871fe6 594 {
simontruelove 11:74eeb8871fe6 595 ReadKType();
simontruelove 11:74eeb8871fe6 596 pc.printf("%f\r",temp);
simontruelove 11:74eeb8871fe6 597 wait(1);
simontruelove 11:74eeb8871fe6 598 if(temp<T-20)
simontruelove 11:74eeb8871fe6 599 {
simontruelove 11:74eeb8871fe6 600 pc.printf("Motor Back Online\n\r");
simontruelove 11:74eeb8871fe6 601 break;
simontruelove 11:74eeb8871fe6 602 }
simontruelove 11:74eeb8871fe6 603 }
simontruelove 11:74eeb8871fe6 604 }
simontruelove 1:0191658b6ff4 605 }
simontruelove 1:0191658b6ff4 606 }
simontruelove 16:e6c8df9960c6 607 void StepACW(void) //Square wave switching
simontruelove 1:0191658b6ff4 608 {
simontruelove 1:0191658b6ff4 609 Ph4();
simontruelove 3:4249dbdf7ed3 610 wait(x);
simontruelove 14:1eb49362a607 611 //pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 612 Ph34();
simontruelove 14:1eb49362a607 613 wait(y);
simontruelove 14:1eb49362a607 614 //pc.printf("34 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 615 Ph3();
simontruelove 14:1eb49362a607 616 wait(x);
simontruelove 14:1eb49362a607 617 //pc.printf("3 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 618 Ph23();
simontruelove 14:1eb49362a607 619 wait(y);
simontruelove 14:1eb49362a607 620 //pc.printf("23 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 621 Ph2();
simontruelove 14:1eb49362a607 622 wait(x);
simontruelove 14:1eb49362a607 623 //pc.printf("2 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 624 Ph12();
simontruelove 14:1eb49362a607 625 wait(y);
simontruelove 14:1eb49362a607 626 //pc.printf("12 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 627 Ph1();
simontruelove 14:1eb49362a607 628 wait(x);
simontruelove 14:1eb49362a607 629 //pc.printf("1 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 630 Ph41();
simontruelove 14:1eb49362a607 631 wait(y);
simontruelove 14:1eb49362a607 632 //pc.printf("41 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 13:da9d3fbbe407 633 }
simontruelove 13:da9d3fbbe407 634
simontruelove 13:da9d3fbbe407 635 void Ph0(void)
simontruelove 13:da9d3fbbe407 636 {
simontruelove 13:da9d3fbbe407 637 Phase1.write(0);
simontruelove 13:da9d3fbbe407 638 Phase2.write(0);
simontruelove 13:da9d3fbbe407 639 Phase3.write(0);
simontruelove 13:da9d3fbbe407 640 Phase4.write(0);
simontruelove 13:da9d3fbbe407 641 //wait(x);
simontruelove 13:da9d3fbbe407 642 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 1:0191658b6ff4 643 }
simontruelove 1:0191658b6ff4 644
simontruelove 1:0191658b6ff4 645 void Ph1(void)
simontruelove 1:0191658b6ff4 646 {
simontruelove 9:061600a6c750 647 Phase1.write(duty);
simontruelove 9:061600a6c750 648 Phase2.write(0);
simontruelove 9:061600a6c750 649 Phase3.write(0);
simontruelove 9:061600a6c750 650 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 651 //wait(x);
simontruelove 3:4249dbdf7ed3 652 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 653 }
simontruelove 3:4249dbdf7ed3 654
simontruelove 3:4249dbdf7ed3 655 void Ph12 (void)
simontruelove 3:4249dbdf7ed3 656 {
simontruelove 9:061600a6c750 657 Phase1.write(duty);
simontruelove 9:061600a6c750 658 Phase2.write(duty);
simontruelove 9:061600a6c750 659 Phase3.write(0);
simontruelove 9:061600a6c750 660 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 661 //wait(y);
simontruelove 1:0191658b6ff4 662 }
simontruelove 1:0191658b6ff4 663
simontruelove 1:0191658b6ff4 664 void Ph2(void)
simontruelove 1:0191658b6ff4 665 {
simontruelove 9:061600a6c750 666 Phase1.write(0);
simontruelove 9:061600a6c750 667 Phase2.write(duty);
simontruelove 9:061600a6c750 668 Phase3.write(0);
simontruelove 9:061600a6c750 669 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 670 //wait(x);
simontruelove 3:4249dbdf7ed3 671 //pc.printf("Phase 2 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 672 }
simontruelove 3:4249dbdf7ed3 673
simontruelove 3:4249dbdf7ed3 674 void Ph23 (void)
simontruelove 3:4249dbdf7ed3 675 {
simontruelove 9:061600a6c750 676 Phase1.write(0);
simontruelove 9:061600a6c750 677 Phase2.write(duty);
simontruelove 9:061600a6c750 678 Phase3.write(duty);
simontruelove 9:061600a6c750 679 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 680 //wait(y);
simontruelove 1:0191658b6ff4 681 }
simontruelove 0:634dd505dace 682
simontruelove 1:0191658b6ff4 683 void Ph3(void)
simontruelove 1:0191658b6ff4 684 {
simontruelove 9:061600a6c750 685 Phase1.write(0);
simontruelove 9:061600a6c750 686 Phase2.write(0);
simontruelove 9:061600a6c750 687 Phase3.write(duty);
simontruelove 9:061600a6c750 688 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 689 //wait(x);
simontruelove 3:4249dbdf7ed3 690 //pc.printf("Phase 3 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 691 }
simontruelove 3:4249dbdf7ed3 692 void Ph34 (void)
simontruelove 3:4249dbdf7ed3 693 {
simontruelove 9:061600a6c750 694 Phase1.write(0);
simontruelove 9:061600a6c750 695 Phase2.write(0);
simontruelove 9:061600a6c750 696 Phase3.write(duty);
simontruelove 9:061600a6c750 697 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 698 //wait(y);
simontruelove 1:0191658b6ff4 699 }
simontruelove 1:0191658b6ff4 700
simontruelove 1:0191658b6ff4 701 void Ph4(void)
simontruelove 1:0191658b6ff4 702 {
simontruelove 9:061600a6c750 703 Phase1.write(0);
simontruelove 9:061600a6c750 704 Phase2.write(0);
simontruelove 9:061600a6c750 705 Phase3.write(0);
simontruelove 9:061600a6c750 706 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 707 //wait(x);
simontruelove 3:4249dbdf7ed3 708 //pc.printf("Phase 4 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 709 }
simontruelove 3:4249dbdf7ed3 710
simontruelove 3:4249dbdf7ed3 711 void Ph41 (void)
simontruelove 3:4249dbdf7ed3 712 {
simontruelove 9:061600a6c750 713 Phase1.write(duty);
simontruelove 9:061600a6c750 714 Phase2.write(0);
simontruelove 9:061600a6c750 715 Phase3.write(0);
simontruelove 9:061600a6c750 716 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 717 //wait(y);
simontruelove 3:4249dbdf7ed3 718 }
simontruelove 3:4249dbdf7ed3 719
simontruelove 10:808cb9052f14 720 void Initialisation (void) //Turn everything off
simontruelove 3:4249dbdf7ed3 721 {
simontruelove 9:061600a6c750 722 Phase1.write(0);
simontruelove 9:061600a6c750 723 Phase2.write(0);
simontruelove 9:061600a6c750 724 Phase3.write(0);
simontruelove 9:061600a6c750 725 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 726 led1 = 0;
simontruelove 3:4249dbdf7ed3 727 led2 = 0;
simontruelove 3:4249dbdf7ed3 728 led3 = 0;
simontruelove 3:4249dbdf7ed3 729 led4 = 0;
simontruelove 12:cbea987a3ec4 730 UnUsedPhase1=0;
simontruelove 12:cbea987a3ec4 731 UnUsedPhase2=0;
simontruelove 3:4249dbdf7ed3 732 wheel.ResetYay();
simontruelove 14:1eb49362a607 733 wheel.QEI::reset();
simontruelove 4:3aedc9246ae4 734 }
simontruelove 4:3aedc9246ae4 735
simontruelove 10:808cb9052f14 736 void GetChar (void) //read keyboard strikes with terraterm
simontruelove 4:3aedc9246ae4 737 { if (pc.readable())
simontruelove 4:3aedc9246ae4 738 {
simontruelove 4:3aedc9246ae4 739 c = pc.getc();
simontruelove 16:e6c8df9960c6 740 if(c ==' ')
simontruelove 16:e6c8df9960c6 741 {
simontruelove 16:e6c8df9960c6 742 pc.printf(" 0 = Phase Mapping\n\rq = setpoint+, w = duty+, e = AdjDiff+, t = temp, y = states, o = AdjCW+, p = AdjACW+,\n\r a = setpoint-, s = duty-, d = AdjDiff-, k = AdjCW-, l = AdjACW-,\n\r z = CW, x = ACW\n\r");
simontruelove 16:e6c8df9960c6 743 }
simontruelove 10:808cb9052f14 744 if(c == 'z') //turn on led1 causes CW operation
simontruelove 4:3aedc9246ae4 745 {
simontruelove 4:3aedc9246ae4 746 led1 = !led1;
simontruelove 4:3aedc9246ae4 747 led2 = 0;
simontruelove 13:da9d3fbbe407 748 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 749 }
simontruelove 10:808cb9052f14 750 if(c == 'x') //turn on led2 causes ACW operation
simontruelove 4:3aedc9246ae4 751 {
simontruelove 4:3aedc9246ae4 752 led1 = 0;
simontruelove 4:3aedc9246ae4 753 led2 = !led2 ;
simontruelove 10:808cb9052f14 754 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 755 }
simontruelove 10:808cb9052f14 756 if(c == 'q') //Increases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 757 {
simontruelove 13:da9d3fbbe407 758 SetPoint=SetPoint+5;
simontruelove 14:1eb49362a607 759 if (SetPoint >3000)
simontruelove 10:808cb9052f14 760 {
simontruelove 14:1eb49362a607 761 SetPoint = 3000;
simontruelove 10:808cb9052f14 762 }
simontruelove 10:808cb9052f14 763 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 7:b8de1529c7fc 764 }
simontruelove 10:808cb9052f14 765 if(c == 'a') //Decreases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 766 {
simontruelove 13:da9d3fbbe407 767 SetPoint=SetPoint-5;
simontruelove 10:808cb9052f14 768 if (SetPoint <50)
simontruelove 9:061600a6c750 769 {
simontruelove 10:808cb9052f14 770 SetPoint = 50;
simontruelove 9:061600a6c750 771 }
simontruelove 10:808cb9052f14 772 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 773 }
simontruelove 14:1eb49362a607 774 if(c == 'w') //Increases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 775 {
simontruelove 16:e6c8df9960c6 776 duty = duty + 0.01;
simontruelove 16:e6c8df9960c6 777 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 778 }
simontruelove 14:1eb49362a607 779 if(c == 's') //Decreases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 780 {
simontruelove 16:e6c8df9960c6 781 duty = duty - 0.01;
simontruelove 16:e6c8df9960c6 782 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 783 }
simontruelove 14:1eb49362a607 784 if(c == 'e') //Increases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 785 {
simontruelove 14:1eb49362a607 786 AdjDiff = AdjDiff * 10;
simontruelove 16:e6c8df9960c6 787 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 788 }
simontruelove 14:1eb49362a607 789 if(c == 'd') //Decreases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 790 {
simontruelove 14:1eb49362a607 791 AdjDiff = AdjDiff / 10;
simontruelove 16:e6c8df9960c6 792 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 793 }
simontruelove 10:808cb9052f14 794 if(c== 'o')
simontruelove 10:808cb9052f14 795 {
simontruelove 10:808cb9052f14 796 AdjCW = AdjCW+1;
simontruelove 14:1eb49362a607 797 if (AdjCW >(s-1))
simontruelove 10:808cb9052f14 798 {
simontruelove 10:808cb9052f14 799 AdjCW = 0;
simontruelove 10:808cb9052f14 800 }
simontruelove 10:808cb9052f14 801 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 802 }
simontruelove 10:808cb9052f14 803 if(c== 'k')
simontruelove 10:808cb9052f14 804 {
simontruelove 10:808cb9052f14 805 AdjCW = AdjCW-1;
simontruelove 10:808cb9052f14 806 if (AdjCW <0)
simontruelove 10:808cb9052f14 807 {
simontruelove 14:1eb49362a607 808 AdjCW = (s-1);
simontruelove 10:808cb9052f14 809 }
simontruelove 10:808cb9052f14 810 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 811 }
simontruelove 10:808cb9052f14 812 if(c== 'p')
simontruelove 10:808cb9052f14 813 {
simontruelove 10:808cb9052f14 814 AdjACW = AdjACW+1;
simontruelove 14:1eb49362a607 815 if (AdjACW >(s-1))
simontruelove 10:808cb9052f14 816 {
simontruelove 10:808cb9052f14 817 AdjACW = 0;
simontruelove 10:808cb9052f14 818 }
simontruelove 10:808cb9052f14 819 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 820 }
simontruelove 10:808cb9052f14 821 if(c== 'l')
simontruelove 10:808cb9052f14 822 {
simontruelove 10:808cb9052f14 823 AdjACW = AdjACW-1;
simontruelove 10:808cb9052f14 824 if (AdjACW <0)
simontruelove 10:808cb9052f14 825 {
simontruelove 14:1eb49362a607 826 AdjACW = (s-1);
simontruelove 10:808cb9052f14 827 }
simontruelove 10:808cb9052f14 828 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 829 }
simontruelove 16:e6c8df9960c6 830 if(c== 'i')
simontruelove 16:e6c8df9960c6 831 {
simontruelove 16:e6c8df9960c6 832 n = n+1;
simontruelove 16:e6c8df9960c6 833 /*if (n > 63)
simontruelove 16:e6c8df9960c6 834 {
simontruelove 16:e6c8df9960c6 835 n = 0;
simontruelove 16:e6c8df9960c6 836 }*/
simontruelove 16:e6c8df9960c6 837 pc.printf("%i, %.5f, %i, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n);
simontruelove 16:e6c8df9960c6 838 }
simontruelove 16:e6c8df9960c6 839 if(c== 'j')
simontruelove 16:e6c8df9960c6 840 {
simontruelove 16:e6c8df9960c6 841 n = n-1;
simontruelove 16:e6c8df9960c6 842 /*if (n <0)
simontruelove 16:e6c8df9960c6 843 {
simontruelove 16:e6c8df9960c6 844 n = 63;
simontruelove 16:e6c8df9960c6 845 }*/
simontruelove 16:e6c8df9960c6 846 pc.printf("%i, %.5f, %i, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n);
simontruelove 16:e6c8df9960c6 847 }
simontruelove 16:e6c8df9960c6 848 if(c== 'r')
simontruelove 16:e6c8df9960c6 849 {
simontruelove 16:e6c8df9960c6 850 p = p+0.0000001;
simontruelove 16:e6c8df9960c6 851 pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);
simontruelove 16:e6c8df9960c6 852 }
simontruelove 16:e6c8df9960c6 853 if(c== 'f')
simontruelove 16:e6c8df9960c6 854 {
simontruelove 16:e6c8df9960c6 855 p = p-0.0000001;
simontruelove 16:e6c8df9960c6 856 pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);
simontruelove 16:e6c8df9960c6 857 }
simontruelove 10:808cb9052f14 858 if(c=='0')
simontruelove 10:808cb9052f14 859 {
simontruelove 16:e6c8df9960c6 860 pc.printf("%i, %.5f, %i, %i, %i, %i, %f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n, AdjDiff, p);
simontruelove 8:2e690f407ec6 861 }
simontruelove 11:74eeb8871fe6 862 if(c=='t')
simontruelove 11:74eeb8871fe6 863 {
simontruelove 12:cbea987a3ec4 864 pc.printf("%.0f C\n\r",temp);
simontruelove 11:74eeb8871fe6 865 }
simontruelove 14:1eb49362a607 866 if(c=='y')
simontruelove 14:1eb49362a607 867 {
simontruelove 16:e6c8df9960c6 868 pc.printf("State A = %i, StateB = %i, Pulses= %i, Revs= %i \n\r", StateA, StateB, wheel.getPulses(), wheel.getRevolutions());
simontruelove 14:1eb49362a607 869 }
simontruelove 4:3aedc9246ae4 870 }
simontruelove 5:4e5c644d5cc3 871 }
simontruelove 5:4e5c644d5cc3 872
simontruelove 5:4e5c644d5cc3 873 void RPM (void)
simontruelove 5:4e5c644d5cc3 874 {
simontruelove 14:1eb49362a607 875 wheel.ResetWhoop(); //PulseCount2_==400 x 4, x4 encoding, whoop_=1;
simontruelove 14:1eb49362a607 876 TimePerClick = (t.read_us()); //read timer in microseconds
simontruelove 14:1eb49362a607 877 t.reset(); //reset timer
simontruelove 14:1eb49362a607 878 TimePerRev = enc * TimePerClick / z; //z = 400 (PulseCount2_==1600)
simontruelove 14:1eb49362a607 879 TimePerRev = TimePerRev / 1000000; // 1microsecond = 0.000001s
simontruelove 14:1eb49362a607 880 RPS = 1 / TimePerRev; //inverse to convert SPR to RPS
simontruelove 14:1eb49362a607 881 rpm = RPS * 60; //x 60 to convert RPS to RPM
simontruelove 14:1eb49362a607 882 //Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
simontruelove 14:1eb49362a607 883 //ReadKType();
simontruelove 16:e6c8df9960c6 884 if(duty < 0.7)
simontruelove 14:1eb49362a607 885 {
simontruelove 16:e6c8df9960c6 886 if(rpm < 65)
simontruelove 16:e6c8df9960c6 887 {
simontruelove 16:e6c8df9960c6 888 AdjCW = (57);
simontruelove 16:e6c8df9960c6 889 AdjACW = (12);
simontruelove 16:e6c8df9960c6 890 }
simontruelove 16:e6c8df9960c6 891 if(rpm > 64 and rpm <75)
simontruelove 16:e6c8df9960c6 892 {
simontruelove 16:e6c8df9960c6 893 AdjCW = (57);
simontruelove 16:e6c8df9960c6 894 AdjACW = (12);
simontruelove 16:e6c8df9960c6 895 }
simontruelove 16:e6c8df9960c6 896 if(rpm > 74 and rpm < 90)
simontruelove 16:e6c8df9960c6 897 {
simontruelove 16:e6c8df9960c6 898 AdjCW = (60);
simontruelove 16:e6c8df9960c6 899 AdjACW = (9);
simontruelove 16:e6c8df9960c6 900 }
simontruelove 16:e6c8df9960c6 901 if(rpm > 89 and rpm < 200)
simontruelove 16:e6c8df9960c6 902 {
simontruelove 16:e6c8df9960c6 903 AdjCW = (60);
simontruelove 16:e6c8df9960c6 904 AdjACW = (9);
simontruelove 16:e6c8df9960c6 905 }
simontruelove 16:e6c8df9960c6 906 if(rpm > 199 and rpm < 300)
simontruelove 16:e6c8df9960c6 907 {
simontruelove 16:e6c8df9960c6 908 AdjCW = (61);
simontruelove 16:e6c8df9960c6 909 AdjACW = (8);
simontruelove 16:e6c8df9960c6 910 }
simontruelove 16:e6c8df9960c6 911 if(rpm > 299 and rpm <400)
simontruelove 16:e6c8df9960c6 912 {
simontruelove 16:e6c8df9960c6 913 AdjCW = (62);
simontruelove 16:e6c8df9960c6 914 AdjACW = (7);
simontruelove 16:e6c8df9960c6 915 }
simontruelove 16:e6c8df9960c6 916 if(rpm > 399 and rpm < 525)
simontruelove 16:e6c8df9960c6 917 {
simontruelove 16:e6c8df9960c6 918 AdjCW = (63);
simontruelove 16:e6c8df9960c6 919 AdjACW = (6);
simontruelove 16:e6c8df9960c6 920 }
simontruelove 16:e6c8df9960c6 921 if(rpm > 524 and rpm < 625)
simontruelove 16:e6c8df9960c6 922 {
simontruelove 16:e6c8df9960c6 923 AdjCW = (0);
simontruelove 16:e6c8df9960c6 924 AdjACW = (5);
simontruelove 16:e6c8df9960c6 925 }
simontruelove 16:e6c8df9960c6 926 if(rpm > 624 and rpm < 725)
simontruelove 16:e6c8df9960c6 927 {
simontruelove 16:e6c8df9960c6 928 AdjCW = (1);
simontruelove 16:e6c8df9960c6 929 AdjACW = (4);
simontruelove 16:e6c8df9960c6 930 }
simontruelove 16:e6c8df9960c6 931 if(rpm > 724 and rpm < 850)
simontruelove 16:e6c8df9960c6 932 {
simontruelove 16:e6c8df9960c6 933 AdjCW = (2);
simontruelove 16:e6c8df9960c6 934 AdjACW = (3);
simontruelove 16:e6c8df9960c6 935 }
simontruelove 16:e6c8df9960c6 936 if(rpm > 849 and rpm < 975)
simontruelove 16:e6c8df9960c6 937 {
simontruelove 16:e6c8df9960c6 938 AdjCW = (3);
simontruelove 16:e6c8df9960c6 939 AdjACW = (2);
simontruelove 16:e6c8df9960c6 940 }
simontruelove 16:e6c8df9960c6 941 if(rpm > 974 and rpm < 1150)
simontruelove 16:e6c8df9960c6 942 {
simontruelove 16:e6c8df9960c6 943 AdjCW = (4);
simontruelove 16:e6c8df9960c6 944 AdjACW = (1-n);
simontruelove 16:e6c8df9960c6 945 }
simontruelove 16:e6c8df9960c6 946 if(rpm > 1149 and rpm < 1375)
simontruelove 16:e6c8df9960c6 947 {
simontruelove 16:e6c8df9960c6 948 AdjCW = (5);
simontruelove 16:e6c8df9960c6 949 AdjACW = (0);
simontruelove 16:e6c8df9960c6 950 }
simontruelove 16:e6c8df9960c6 951 if(rpm > 1374 and rpm < 1725)
simontruelove 16:e6c8df9960c6 952 {
simontruelove 16:e6c8df9960c6 953 AdjCW = (6);
simontruelove 16:e6c8df9960c6 954 AdjACW = (63);
simontruelove 16:e6c8df9960c6 955 }
simontruelove 16:e6c8df9960c6 956 if(rpm > 1724 and rpm < 2150)
simontruelove 16:e6c8df9960c6 957 {
simontruelove 16:e6c8df9960c6 958 AdjCW = (7);
simontruelove 16:e6c8df9960c6 959 AdjACW = (62);
simontruelove 16:e6c8df9960c6 960 }
simontruelove 16:e6c8df9960c6 961 if(rpm > 2149 and rpm < 2525)
simontruelove 16:e6c8df9960c6 962 {
simontruelove 16:e6c8df9960c6 963 AdjCW = (8);
simontruelove 16:e6c8df9960c6 964 AdjACW = (61);
simontruelove 16:e6c8df9960c6 965 }
simontruelove 16:e6c8df9960c6 966 if(rpm > 2524 and rpm < 2600)
simontruelove 16:e6c8df9960c6 967 {
simontruelove 16:e6c8df9960c6 968 AdjCW = (9);
simontruelove 16:e6c8df9960c6 969 AdjACW = (60);
simontruelove 16:e6c8df9960c6 970 }
simontruelove 16:e6c8df9960c6 971 if(rpm > 2599 and rpm < 2650)
simontruelove 16:e6c8df9960c6 972 {
simontruelove 16:e6c8df9960c6 973 AdjCW = (10);
simontruelove 16:e6c8df9960c6 974 AdjACW = (59);
simontruelove 16:e6c8df9960c6 975 }
simontruelove 16:e6c8df9960c6 976 if(rpm > 2649 and rpm < 2700)
simontruelove 16:e6c8df9960c6 977 {
simontruelove 16:e6c8df9960c6 978 AdjCW = (11);
simontruelove 16:e6c8df9960c6 979 AdjACW = (58);
simontruelove 16:e6c8df9960c6 980 }
simontruelove 16:e6c8df9960c6 981 if(rpm > 2699 and rpm < 2750)
simontruelove 16:e6c8df9960c6 982 {
simontruelove 16:e6c8df9960c6 983 AdjCW = (12);
simontruelove 16:e6c8df9960c6 984 AdjACW = (57);
simontruelove 16:e6c8df9960c6 985 }
simontruelove 16:e6c8df9960c6 986 if(rpm > 2749 and rpm < 2825)
simontruelove 16:e6c8df9960c6 987 {
simontruelove 16:e6c8df9960c6 988 AdjCW = (13);
simontruelove 16:e6c8df9960c6 989 AdjACW = (56);
simontruelove 16:e6c8df9960c6 990 }
simontruelove 16:e6c8df9960c6 991 if(rpm > 2824 and rpm < 2875)
simontruelove 16:e6c8df9960c6 992 {
simontruelove 16:e6c8df9960c6 993 AdjCW = (14);
simontruelove 16:e6c8df9960c6 994 AdjACW = (55);
simontruelove 16:e6c8df9960c6 995 }
simontruelove 16:e6c8df9960c6 996 if(rpm > 2874)
simontruelove 16:e6c8df9960c6 997 {
simontruelove 16:e6c8df9960c6 998 AdjCW = (15);
simontruelove 16:e6c8df9960c6 999 AdjACW = (54);
simontruelove 16:e6c8df9960c6 1000 }
simontruelove 14:1eb49362a607 1001 }
simontruelove 16:e6c8df9960c6 1002 if(duty > 0.69)
simontruelove 14:1eb49362a607 1003 {
simontruelove 16:e6c8df9960c6 1004 if(rpm < 175)
simontruelove 16:e6c8df9960c6 1005 {
simontruelove 16:e6c8df9960c6 1006 AdjCW = (60);
simontruelove 16:e6c8df9960c6 1007 AdjACW = (11);
simontruelove 16:e6c8df9960c6 1008 }
simontruelove 16:e6c8df9960c6 1009 if(rpm > 200 and rpm < 275)
simontruelove 16:e6c8df9960c6 1010 {
simontruelove 16:e6c8df9960c6 1011 AdjCW = (5);
simontruelove 16:e6c8df9960c6 1012 AdjACW = (11);
simontruelove 16:e6c8df9960c6 1013 }if(rpm > 300 and rpm < 1000)
simontruelove 16:e6c8df9960c6 1014 {
simontruelove 16:e6c8df9960c6 1015 AdjCW = (5);
simontruelove 16:e6c8df9960c6 1016 AdjACW = (5);
simontruelove 16:e6c8df9960c6 1017 }
simontruelove 14:1eb49362a607 1018 }
simontruelove 16:e6c8df9960c6 1019 /*if(rpm < 180)
simontruelove 13:da9d3fbbe407 1020 {
simontruelove 16:e6c8df9960c6 1021 AdjCW = (57+n);
simontruelove 16:e6c8df9960c6 1022 AdjACW = (12-n);
simontruelove 16:e6c8df9960c6 1023 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1024 {
simontruelove 16:e6c8df9960c6 1025 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1026 }
simontruelove 16:e6c8df9960c6 1027 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1028 {
simontruelove 16:e6c8df9960c6 1029 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1030 }
simontruelove 16:e6c8df9960c6 1031 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1032 {
simontruelove 16:e6c8df9960c6 1033 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1034 }
simontruelove 16:e6c8df9960c6 1035 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1036 {
simontruelove 16:e6c8df9960c6 1037 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1038 }
simontruelove 14:1eb49362a607 1039 }
simontruelove 16:e6c8df9960c6 1040 if(rpm > 179 and rpm <195)
simontruelove 14:1eb49362a607 1041 {
simontruelove 16:e6c8df9960c6 1042 AdjCW = (58+n);
simontruelove 16:e6c8df9960c6 1043 AdjACW = (11-n);
simontruelove 16:e6c8df9960c6 1044 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1045 {
simontruelove 16:e6c8df9960c6 1046 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1047 }
simontruelove 16:e6c8df9960c6 1048 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1049 {
simontruelove 16:e6c8df9960c6 1050 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1051 }
simontruelove 16:e6c8df9960c6 1052 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1053 {
simontruelove 16:e6c8df9960c6 1054 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1055 }
simontruelove 16:e6c8df9960c6 1056 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1057 {
simontruelove 16:e6c8df9960c6 1058 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1059 }
simontruelove 13:da9d3fbbe407 1060 }
simontruelove 16:e6c8df9960c6 1061 if(rpm > 194 and rpm <220)
simontruelove 16:e6c8df9960c6 1062 {
simontruelove 16:e6c8df9960c6 1063 AdjCW = (59+n);
simontruelove 16:e6c8df9960c6 1064 AdjACW = (10-n);
simontruelove 16:e6c8df9960c6 1065 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1066 {
simontruelove 16:e6c8df9960c6 1067 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1068 }
simontruelove 16:e6c8df9960c6 1069 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1070 {
simontruelove 16:e6c8df9960c6 1071 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1072 }
simontruelove 16:e6c8df9960c6 1073 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1074 {
simontruelove 16:e6c8df9960c6 1075 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1076 }
simontruelove 16:e6c8df9960c6 1077 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1078 {
simontruelove 16:e6c8df9960c6 1079 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1080 }
simontruelove 16:e6c8df9960c6 1081 }
simontruelove 16:e6c8df9960c6 1082 if(rpm > 219 and rpm <240)
simontruelove 14:1eb49362a607 1083 {
simontruelove 16:e6c8df9960c6 1084 AdjCW = (60+n);
simontruelove 16:e6c8df9960c6 1085 AdjACW = (9-n);
simontruelove 16:e6c8df9960c6 1086 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1087 {
simontruelove 16:e6c8df9960c6 1088 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1089 }
simontruelove 16:e6c8df9960c6 1090 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1091 {
simontruelove 16:e6c8df9960c6 1092 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1093 }
simontruelove 16:e6c8df9960c6 1094 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1095 {
simontruelove 16:e6c8df9960c6 1096 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1097 }
simontruelove 16:e6c8df9960c6 1098 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1099 {
simontruelove 16:e6c8df9960c6 1100 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1101 }
simontruelove 14:1eb49362a607 1102 }
simontruelove 16:e6c8df9960c6 1103 if(rpm > 239 and rpm <295)
simontruelove 16:e6c8df9960c6 1104 {
simontruelove 16:e6c8df9960c6 1105 AdjCW = (61+n);
simontruelove 16:e6c8df9960c6 1106 AdjACW = (8-n);
simontruelove 16:e6c8df9960c6 1107 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1108 {
simontruelove 16:e6c8df9960c6 1109 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1110 }
simontruelove 16:e6c8df9960c6 1111 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1112 {
simontruelove 16:e6c8df9960c6 1113 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1114 }
simontruelove 16:e6c8df9960c6 1115 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1116 {
simontruelove 16:e6c8df9960c6 1117 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1118 }
simontruelove 16:e6c8df9960c6 1119 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1120 {
simontruelove 16:e6c8df9960c6 1121 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1122 }
simontruelove 16:e6c8df9960c6 1123 }
simontruelove 16:e6c8df9960c6 1124 if(rpm > 294 and rpm <325)
simontruelove 13:da9d3fbbe407 1125 {
simontruelove 16:e6c8df9960c6 1126 AdjCW = (62+n);
simontruelove 16:e6c8df9960c6 1127 AdjACW = (7-n);
simontruelove 16:e6c8df9960c6 1128 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1129 {
simontruelove 16:e6c8df9960c6 1130 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1131 }
simontruelove 16:e6c8df9960c6 1132 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1133 {
simontruelove 16:e6c8df9960c6 1134 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1135 }
simontruelove 16:e6c8df9960c6 1136 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1137 {
simontruelove 16:e6c8df9960c6 1138 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1139 }
simontruelove 16:e6c8df9960c6 1140 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1141 {
simontruelove 16:e6c8df9960c6 1142 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1143 }
simontruelove 13:da9d3fbbe407 1144 }
simontruelove 16:e6c8df9960c6 1145 if(rpm > 324 and rpm <355)
simontruelove 16:e6c8df9960c6 1146 {
simontruelove 16:e6c8df9960c6 1147 AdjCW = (63+n);
simontruelove 16:e6c8df9960c6 1148 AdjACW = (6-n);
simontruelove 16:e6c8df9960c6 1149 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1150 {
simontruelove 16:e6c8df9960c6 1151 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1152 }
simontruelove 16:e6c8df9960c6 1153 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1154 {
simontruelove 16:e6c8df9960c6 1155 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1156 }
simontruelove 16:e6c8df9960c6 1157 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1158 {
simontruelove 16:e6c8df9960c6 1159 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1160 }
simontruelove 16:e6c8df9960c6 1161 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1162 {
simontruelove 16:e6c8df9960c6 1163 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1164 }
simontruelove 16:e6c8df9960c6 1165 }
simontruelove 16:e6c8df9960c6 1166 if(rpm > 354 and rpm <415)
simontruelove 13:da9d3fbbe407 1167 {
simontruelove 16:e6c8df9960c6 1168 AdjCW = (0+n);
simontruelove 16:e6c8df9960c6 1169 AdjACW = (5-n);
simontruelove 16:e6c8df9960c6 1170 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1171 {
simontruelove 16:e6c8df9960c6 1172 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1173 }
simontruelove 16:e6c8df9960c6 1174 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1175 {
simontruelove 16:e6c8df9960c6 1176 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1177 }
simontruelove 16:e6c8df9960c6 1178 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1179 {
simontruelove 16:e6c8df9960c6 1180 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1181 }
simontruelove 16:e6c8df9960c6 1182 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1183 {
simontruelove 16:e6c8df9960c6 1184 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1185 }
simontruelove 16:e6c8df9960c6 1186 }
simontruelove 16:e6c8df9960c6 1187 if(rpm > 414 and rpm <595)
simontruelove 13:da9d3fbbe407 1188 {
simontruelove 16:e6c8df9960c6 1189 AdjCW = (1+n);
simontruelove 16:e6c8df9960c6 1190 AdjACW = (4-n);
simontruelove 16:e6c8df9960c6 1191 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1192 {
simontruelove 16:e6c8df9960c6 1193 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1194 }
simontruelove 16:e6c8df9960c6 1195 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1196 {
simontruelove 16:e6c8df9960c6 1197 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1198 }
simontruelove 16:e6c8df9960c6 1199 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1200 {
simontruelove 16:e6c8df9960c6 1201 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1202 }
simontruelove 16:e6c8df9960c6 1203 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1204 {
simontruelove 16:e6c8df9960c6 1205 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1206 }
simontruelove 14:1eb49362a607 1207 }
simontruelove 16:e6c8df9960c6 1208 if(rpm > 594 and rpm <655)
simontruelove 14:1eb49362a607 1209 {
simontruelove 16:e6c8df9960c6 1210 AdjCW = (2+n);
simontruelove 16:e6c8df9960c6 1211 AdjACW = (3-n);
simontruelove 16:e6c8df9960c6 1212 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1213 {
simontruelove 16:e6c8df9960c6 1214 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1215 }
simontruelove 16:e6c8df9960c6 1216 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1217 {
simontruelove 16:e6c8df9960c6 1218 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1219 }
simontruelove 16:e6c8df9960c6 1220 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1221 {
simontruelove 16:e6c8df9960c6 1222 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1223 }
simontruelove 16:e6c8df9960c6 1224 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1225 {
simontruelove 16:e6c8df9960c6 1226 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1227 }
simontruelove 14:1eb49362a607 1228 }
simontruelove 16:e6c8df9960c6 1229 if(rpm > 654 and rpm <765)
simontruelove 14:1eb49362a607 1230 {
simontruelove 16:e6c8df9960c6 1231 AdjCW = (3+n);
simontruelove 16:e6c8df9960c6 1232 AdjACW = (2-n);
simontruelove 16:e6c8df9960c6 1233 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1234 {
simontruelove 16:e6c8df9960c6 1235 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1236 }
simontruelove 16:e6c8df9960c6 1237 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1238 {
simontruelove 16:e6c8df9960c6 1239 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1240 }
simontruelove 16:e6c8df9960c6 1241 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1242 {
simontruelove 16:e6c8df9960c6 1243 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1244 }
simontruelove 16:e6c8df9960c6 1245 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1246 {
simontruelove 16:e6c8df9960c6 1247 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1248 }
simontruelove 13:da9d3fbbe407 1249 }
simontruelove 16:e6c8df9960c6 1250 if(rpm > 764 and rpm <1010)
simontruelove 16:e6c8df9960c6 1251 {
simontruelove 16:e6c8df9960c6 1252 AdjCW = (4+n);
simontruelove 16:e6c8df9960c6 1253 AdjACW = (1-n);
simontruelove 16:e6c8df9960c6 1254 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1255 {
simontruelove 16:e6c8df9960c6 1256 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1257 }
simontruelove 16:e6c8df9960c6 1258 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1259 {
simontruelove 16:e6c8df9960c6 1260 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1261 }
simontruelove 16:e6c8df9960c6 1262 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1263 {
simontruelove 16:e6c8df9960c6 1264 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1265 }
simontruelove 16:e6c8df9960c6 1266 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1267 {
simontruelove 16:e6c8df9960c6 1268 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1269 }
simontruelove 16:e6c8df9960c6 1270 }
simontruelove 16:e6c8df9960c6 1271 if(rpm > 1009 and rpm <1150)
simontruelove 14:1eb49362a607 1272 {
simontruelove 16:e6c8df9960c6 1273 AdjCW = (5+n);
simontruelove 16:e6c8df9960c6 1274 AdjACW = (0-n);
simontruelove 16:e6c8df9960c6 1275 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1276 {
simontruelove 16:e6c8df9960c6 1277 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1278 }
simontruelove 16:e6c8df9960c6 1279 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1280 {
simontruelove 16:e6c8df9960c6 1281 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1282 }
simontruelove 16:e6c8df9960c6 1283 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1284 {
simontruelove 16:e6c8df9960c6 1285 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1286 }
simontruelove 16:e6c8df9960c6 1287 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1288 {
simontruelove 16:e6c8df9960c6 1289 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1290 }
simontruelove 14:1eb49362a607 1291 }
simontruelove 16:e6c8df9960c6 1292 if(rpm > 1149 and rpm <1225)
simontruelove 16:e6c8df9960c6 1293 {
simontruelove 16:e6c8df9960c6 1294 AdjCW = (6+n);
simontruelove 16:e6c8df9960c6 1295 AdjACW = (63-n);
simontruelove 16:e6c8df9960c6 1296 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1297 {
simontruelove 16:e6c8df9960c6 1298 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1299 }
simontruelove 16:e6c8df9960c6 1300 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1301 {
simontruelove 16:e6c8df9960c6 1302 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1303 }
simontruelove 16:e6c8df9960c6 1304 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1305 {
simontruelove 16:e6c8df9960c6 1306 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1307 }
simontruelove 16:e6c8df9960c6 1308 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1309 {
simontruelove 16:e6c8df9960c6 1310 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1311 }
simontruelove 16:e6c8df9960c6 1312 }
simontruelove 16:e6c8df9960c6 1313 if(rpm > 1224 and rpm <1310)
simontruelove 14:1eb49362a607 1314 {
simontruelove 16:e6c8df9960c6 1315 AdjCW = (7+n);
simontruelove 16:e6c8df9960c6 1316 AdjACW = (62-n);
simontruelove 16:e6c8df9960c6 1317 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1318 {
simontruelove 16:e6c8df9960c6 1319 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1320 }
simontruelove 16:e6c8df9960c6 1321 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1322 {
simontruelove 16:e6c8df9960c6 1323 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1324 }
simontruelove 16:e6c8df9960c6 1325 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1326 {
simontruelove 16:e6c8df9960c6 1327 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1328 }
simontruelove 16:e6c8df9960c6 1329 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1330 {
simontruelove 16:e6c8df9960c6 1331 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1332 }
simontruelove 14:1eb49362a607 1333 }
simontruelove 16:e6c8df9960c6 1334 if(rpm > 1309 and rpm <1670)
simontruelove 13:da9d3fbbe407 1335 {
simontruelove 16:e6c8df9960c6 1336 AdjCW = (8+n);
simontruelove 16:e6c8df9960c6 1337 AdjACW = (61-n);
simontruelove 16:e6c8df9960c6 1338 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1339 {
simontruelove 16:e6c8df9960c6 1340 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1341 }
simontruelove 16:e6c8df9960c6 1342 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1343 {
simontruelove 16:e6c8df9960c6 1344 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1345 }
simontruelove 16:e6c8df9960c6 1346 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1347 {
simontruelove 16:e6c8df9960c6 1348 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1349 }
simontruelove 16:e6c8df9960c6 1350 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1351 {
simontruelove 16:e6c8df9960c6 1352 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1353 }
simontruelove 16:e6c8df9960c6 1354 }
simontruelove 16:e6c8df9960c6 1355 if(rpm > 1669 and rpm <2035)
simontruelove 16:e6c8df9960c6 1356 {
simontruelove 16:e6c8df9960c6 1357 AdjCW = (9+n);
simontruelove 16:e6c8df9960c6 1358 AdjACW = (60-n);
simontruelove 16:e6c8df9960c6 1359 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1360 {
simontruelove 16:e6c8df9960c6 1361 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1362 }
simontruelove 16:e6c8df9960c6 1363 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1364 {
simontruelove 16:e6c8df9960c6 1365 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1366 }
simontruelove 16:e6c8df9960c6 1367 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1368 {
simontruelove 16:e6c8df9960c6 1369 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1370 }
simontruelove 16:e6c8df9960c6 1371 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1372 {
simontruelove 16:e6c8df9960c6 1373 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1374 }
simontruelove 14:1eb49362a607 1375 }
simontruelove 16:e6c8df9960c6 1376 if(rpm > 2034 and rpm <2175)
simontruelove 14:1eb49362a607 1377 {
simontruelove 16:e6c8df9960c6 1378 AdjCW = (10+n);
simontruelove 16:e6c8df9960c6 1379 AdjACW = (59-n);
simontruelove 16:e6c8df9960c6 1380 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1381 {
simontruelove 16:e6c8df9960c6 1382 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1383 }
simontruelove 16:e6c8df9960c6 1384 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1385 {
simontruelove 16:e6c8df9960c6 1386 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1387 }
simontruelove 16:e6c8df9960c6 1388 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1389 {
simontruelove 16:e6c8df9960c6 1390 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1391 }
simontruelove 16:e6c8df9960c6 1392 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1393 {
simontruelove 16:e6c8df9960c6 1394 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1395 }
simontruelove 14:1eb49362a607 1396 }
simontruelove 16:e6c8df9960c6 1397 if(rpm > 2174 and rpm <2275)
simontruelove 14:1eb49362a607 1398 {
simontruelove 16:e6c8df9960c6 1399 AdjCW = (11+n);
simontruelove 16:e6c8df9960c6 1400 AdjACW = (58-n);
simontruelove 16:e6c8df9960c6 1401 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1402 {
simontruelove 16:e6c8df9960c6 1403 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1404 }
simontruelove 16:e6c8df9960c6 1405 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1406 {
simontruelove 16:e6c8df9960c6 1407 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1408 }
simontruelove 16:e6c8df9960c6 1409 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1410 {
simontruelove 16:e6c8df9960c6 1411 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1412 }
simontruelove 16:e6c8df9960c6 1413 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1414 {
simontruelove 16:e6c8df9960c6 1415 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1416 }
simontruelove 13:da9d3fbbe407 1417 }
simontruelove 16:e6c8df9960c6 1418 if(rpm > 2274 and rpm <2515)
simontruelove 16:e6c8df9960c6 1419 {
simontruelove 16:e6c8df9960c6 1420 AdjCW = (12+n);
simontruelove 16:e6c8df9960c6 1421 AdjACW = (57-n);
simontruelove 16:e6c8df9960c6 1422 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1423 {
simontruelove 16:e6c8df9960c6 1424 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1425 }
simontruelove 16:e6c8df9960c6 1426 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1427 {
simontruelove 16:e6c8df9960c6 1428 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1429 }
simontruelove 16:e6c8df9960c6 1430 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1431 {
simontruelove 16:e6c8df9960c6 1432 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1433 }
simontruelove 16:e6c8df9960c6 1434 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1435 {
simontruelove 16:e6c8df9960c6 1436 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1437 }
simontruelove 16:e6c8df9960c6 1438 }
simontruelove 16:e6c8df9960c6 1439 if(rpm > 2514 and rpm <2575)
simontruelove 14:1eb49362a607 1440 {
simontruelove 16:e6c8df9960c6 1441 AdjCW = (13+n);
simontruelove 16:e6c8df9960c6 1442 AdjACW = (56-n);
simontruelove 16:e6c8df9960c6 1443 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1444 {
simontruelove 16:e6c8df9960c6 1445 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1446 }
simontruelove 16:e6c8df9960c6 1447 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1448 {
simontruelove 16:e6c8df9960c6 1449 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1450 }
simontruelove 16:e6c8df9960c6 1451 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1452 {
simontruelove 16:e6c8df9960c6 1453 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1454 }
simontruelove 16:e6c8df9960c6 1455 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1456 {
simontruelove 16:e6c8df9960c6 1457 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1458 }
simontruelove 14:1eb49362a607 1459 }
simontruelove 16:e6c8df9960c6 1460 if(rpm > 2574 and rpm <2755)
simontruelove 16:e6c8df9960c6 1461 {
simontruelove 16:e6c8df9960c6 1462 AdjCW = (14+n);
simontruelove 16:e6c8df9960c6 1463 AdjACW = (55-n);
simontruelove 16:e6c8df9960c6 1464 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1465 {
simontruelove 16:e6c8df9960c6 1466 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1467 }
simontruelove 16:e6c8df9960c6 1468 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1469 {
simontruelove 16:e6c8df9960c6 1470 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1471 }
simontruelove 16:e6c8df9960c6 1472 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1473 {
simontruelove 16:e6c8df9960c6 1474 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1475 }
simontruelove 16:e6c8df9960c6 1476 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1477 {
simontruelove 16:e6c8df9960c6 1478 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1479 }
simontruelove 16:e6c8df9960c6 1480 }
simontruelove 16:e6c8df9960c6 1481 if(rpm > 2754 and rpm <2835)
simontruelove 14:1eb49362a607 1482 {
simontruelove 16:e6c8df9960c6 1483 AdjCW = (15+n);
simontruelove 16:e6c8df9960c6 1484 AdjACW = (54-n);
simontruelove 16:e6c8df9960c6 1485 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1486 {
simontruelove 16:e6c8df9960c6 1487 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1488 }
simontruelove 16:e6c8df9960c6 1489 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1490 {
simontruelove 16:e6c8df9960c6 1491 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1492 }
simontruelove 16:e6c8df9960c6 1493 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1494 {
simontruelove 16:e6c8df9960c6 1495 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1496 }
simontruelove 16:e6c8df9960c6 1497 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1498 {
simontruelove 16:e6c8df9960c6 1499 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1500 }
simontruelove 14:1eb49362a607 1501 }
simontruelove 16:e6c8df9960c6 1502 if(rpm > 2834 and rpm <2860)
simontruelove 14:1eb49362a607 1503 {
simontruelove 16:e6c8df9960c6 1504 AdjCW = (16+n);
simontruelove 16:e6c8df9960c6 1505 AdjACW = (53-n);
simontruelove 16:e6c8df9960c6 1506 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1507 {
simontruelove 16:e6c8df9960c6 1508 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1509 }
simontruelove 16:e6c8df9960c6 1510 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1511 {
simontruelove 16:e6c8df9960c6 1512 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1513 }
simontruelove 16:e6c8df9960c6 1514 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1515 {
simontruelove 16:e6c8df9960c6 1516 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1517 }
simontruelove 16:e6c8df9960c6 1518 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1519 {
simontruelove 16:e6c8df9960c6 1520 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1521 }
simontruelove 15:12a4bbfa6de4 1522 }
simontruelove 16:e6c8df9960c6 1523 if(rpm > 2861)
simontruelove 16:e6c8df9960c6 1524 {
simontruelove 16:e6c8df9960c6 1525 AdjCW = (17+n);
simontruelove 16:e6c8df9960c6 1526 AdjACW = (52-n);
simontruelove 16:e6c8df9960c6 1527 if (AdjCW >(s-1))
simontruelove 16:e6c8df9960c6 1528 {
simontruelove 16:e6c8df9960c6 1529 AdjCW = 0;
simontruelove 16:e6c8df9960c6 1530 }
simontruelove 16:e6c8df9960c6 1531 if (AdjCW <0)
simontruelove 16:e6c8df9960c6 1532 {
simontruelove 16:e6c8df9960c6 1533 AdjCW = (s-1);
simontruelove 16:e6c8df9960c6 1534 }
simontruelove 16:e6c8df9960c6 1535 if (AdjACW <0)
simontruelove 16:e6c8df9960c6 1536 {
simontruelove 16:e6c8df9960c6 1537 AdjACW = (s-1);
simontruelove 16:e6c8df9960c6 1538 }
simontruelove 16:e6c8df9960c6 1539 if (AdjACW >(s-1))
simontruelove 16:e6c8df9960c6 1540 {
simontruelove 16:e6c8df9960c6 1541 AdjACW = 0;
simontruelove 16:e6c8df9960c6 1542 }
simontruelove 16:e6c8df9960c6 1543 }*/
simontruelove 14:1eb49362a607 1544 //pc.printf("RPS = %i \r", RPS);
simontruelove 6:f7028034aabb 1545 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 7:b8de1529c7fc 1546 }
simontruelove 7:b8de1529c7fc 1547
simontruelove 7:b8de1529c7fc 1548 void VelocityLoop (void)
simontruelove 7:b8de1529c7fc 1549 {
simontruelove 14:1eb49362a607 1550 diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
simontruelove 14:1eb49362a607 1551 duty = duty + (diff*AdjDiff); //0.00001 when change required at fast rpm. 0.001 when changes required at slow rpm. duty is adjusted to speed up or slow down until difference = 0
simontruelove 16:e6c8df9960c6 1552 /*if (duty < 0.8)
simontruelove 14:1eb49362a607 1553 {
simontruelove 16:e6c8df9960c6 1554 j = 0;
simontruelove 16:e6c8df9960c6 1555 k = 0;
simontruelove 16:e6c8df9960c6 1556 l = 0;
simontruelove 16:e6c8df9960c6 1557 m = 0;
simontruelove 16:e6c8df9960c6 1558 n = 0;
simontruelove 14:1eb49362a607 1559 }
simontruelove 16:e6c8df9960c6 1560 if (duty > 0.79)
simontruelove 14:1eb49362a607 1561 {
simontruelove 16:e6c8df9960c6 1562 i = n-5;
simontruelove 16:e6c8df9960c6 1563 j = n-4;
simontruelove 16:e6c8df9960c6 1564 k = n-3;
simontruelove 16:e6c8df9960c6 1565 l = n-2;
simontruelove 16:e6c8df9960c6 1566 m = n-1;
simontruelove 16:e6c8df9960c6 1567 n = 6;
simontruelove 14:1eb49362a607 1568 }*/
simontruelove 14:1eb49362a607 1569 if (duty > 1) //limits for duty. Motor will not operate below 0.96. 1 = max
simontruelove 14:1eb49362a607 1570 {
simontruelove 9:061600a6c750 1571 duty = 1;
simontruelove 7:b8de1529c7fc 1572 }
simontruelove 16:e6c8df9960c6 1573 if (duty <0.08) //3A min duty 0.96, 6.5A min duty 0.4
simontruelove 10:808cb9052f14 1574 {
simontruelove 16:e6c8df9960c6 1575 duty = 0.08;
simontruelove 7:b8de1529c7fc 1576 }
simontruelove 10:808cb9052f14 1577 //pc.printf("%i, %.5f, %i, %i, %i \r", SetPoint, duty, AdjCW, AdjACW, rpm); //SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
simontruelove 11:74eeb8871fe6 1578 }
simontruelove 11:74eeb8871fe6 1579
simontruelove 11:74eeb8871fe6 1580 void ReadKType(void)
simontruelove 11:74eeb8871fe6 1581 {
simontruelove 11:74eeb8871fe6 1582 int i = 0;
simontruelove 11:74eeb8871fe6 1583 int Readout = 0;
simontruelove 11:74eeb8871fe6 1584
simontruelove 11:74eeb8871fe6 1585 cs1 = 0;
simontruelove 14:1eb49362a607 1586 SerialClock = 0; //set clock to 0
simontruelove 16:e6c8df9960c6 1587 wait_ms(0.01);
simontruelove 14:1eb49362a607 1588 SerialClock = 1; //clock once to set to the 13 bit temp data
simontruelove 16:e6c8df9960c6 1589 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1590 SerialClock = 0;
simontruelove 16:e6c8df9960c6 1591 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1592
simontruelove 11:74eeb8871fe6 1593 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 1594 {
simontruelove 11:74eeb8871fe6 1595 if(DOut == 1) //check data, store results in readout
simontruelove 11:74eeb8871fe6 1596 {
simontruelove 12:cbea987a3ec4 1597 Readout |= (1<<i);
simontruelove 11:74eeb8871fe6 1598 }
simontruelove 11:74eeb8871fe6 1599 else
simontruelove 11:74eeb8871fe6 1600 {
simontruelove 11:74eeb8871fe6 1601 Readout |= (0<<i);
simontruelove 11:74eeb8871fe6 1602 }
simontruelove 12:cbea987a3ec4 1603
simontruelove 11:74eeb8871fe6 1604 SerialClock = 1; //clock to the next bit
simontruelove 16:e6c8df9960c6 1605 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1606 SerialClock = 0;
simontruelove 16:e6c8df9960c6 1607 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1608 }
simontruelove 11:74eeb8871fe6 1609 temp = Readout * 0.125; //get the real temp value which is a float
simontruelove 16:e6c8df9960c6 1610 //pc.printf("%.1f\r",temp);
simontruelove 16:e6c8df9960c6 1611 //pc.printf("%i, %.4f\n\r", rpm, duty);
simontruelove 11:74eeb8871fe6 1612 Readout = 0;
simontruelove 11:74eeb8871fe6 1613 cs1 = 1;
simontruelove 3:4249dbdf7ed3 1614 }