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@18:3493de6fe8ce, 2019-04-03 (annotated)

Committer:
simontruelove
Date:
Wed Apr 03 13:31:43 2019 +0000
Revision:
18:3493de6fe8ce
Parent:
17:19b2c598810a
Child:
19:d9ba2f225f39
Improved RPM measurement;

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 17:19b2c598810a 26 DigitalOut UnUsedPhase1 (p21);
simontruelove 17:19b2c598810a 27 PwmOut Phase1 (p22); //Pin and LED set up - originally standard pins but changed to PWM to enable speed control
simontruelove 17:19b2c598810a 28 PwmOut Phase2 (p23);
simontruelove 17:19b2c598810a 29 PwmOut Phase3 (p24);
simontruelove 17:19b2c598810a 30 PwmOut Phase4 (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 17:19b2c598810a 54 int SetPoint = 1000; //for adjusting the speed
simontruelove 17:19b2c598810a 55 int enc = 3200; //800 x4 enc = 3200 Pulses Per Rev
simontruelove 16:e6c8df9960c6 56 int i = 0;
simontruelove 16:e6c8df9960c6 57 int j = 0;
simontruelove 16:e6c8df9960c6 58 int k = 0;
simontruelove 16:e6c8df9960c6 59 int l = 0;
simontruelove 16:e6c8df9960c6 60 int m = 0;
simontruelove 16:e6c8df9960c6 61 int n = 0;
simontruelove 14:1eb49362a607 62 int s = enc/50;
simontruelove 17:19b2c598810a 63 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.
simontruelove 11:74eeb8871fe6 64 int T = 80; //Motor temp limit
simontruelove 14:1eb49362a607 65 int slowloop = 0;
simontruelove 18:3493de6fe8ce 66 int StartUp = 1;
simontruelove 11:74eeb8871fe6 67
simontruelove 10:808cb9052f14 68 char c; //keyboard cotrol GetChar
simontruelove 4:3aedc9246ae4 69
simontruelove 18:3493de6fe8ce 70 float RPS = 0; //for calc of RPM
simontruelove 18:3493de6fe8ce 71 float rpm = 0; //for calc of RPM
simontruelove 17:19b2c598810a 72 float duty = 1;
simontruelove 10:808cb9052f14 73 float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
simontruelove 17:19b2c598810a 74 float AdjDiff = 0.0001;
simontruelove 16:e6c8df9960c6 75 float p = 0.000014;
simontruelove 10:808cb9052f14 76 float x=0.1; //x=time of square wave when 1 phase energised,
simontruelove 14:1eb49362a607 77 float TimePerRev = 0; //for calc of RPM
simontruelove 10:808cb9052f14 78 float y=0.04; //y=time of square wave when 2 phases energised
simontruelove 10:808cb9052f14 79 float temp = 0; //ReadKType
simontruelove 1:0191658b6ff4 80
simontruelove 1:0191658b6ff4 81 int main(void)
simontruelove 3:4249dbdf7ed3 82 {
simontruelove 17:19b2c598810a 83 pc.baud(921600); //Set fastest baud rate
simontruelove 16:e6c8df9960c6 84 Phase1.period(p); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
simontruelove 16:e6c8df9960c6 85 Phase2.period(p);
simontruelove 16:e6c8df9960c6 86 Phase3.period(p);
simontruelove 16:e6c8df9960c6 87 Phase4.period(p);
simontruelove 14:1eb49362a607 88 wait(0.1);
simontruelove 14:1eb49362a607 89 t.start();
simontruelove 14:1eb49362a607 90 SerialClock = 0;
simontruelove 16:e6c8df9960c6 91 StepACW();
simontruelove 3:4249dbdf7ed3 92 Initialisation();
simontruelove 14:1eb49362a607 93
simontruelove 14:1eb49362a607 94 /*while(1)
simontruelove 17:19b2c598810a 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 18:3493de6fe8ce 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 18:3493de6fe8ce 611 pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 612 //Ph34();
simontruelove 18:3493de6fe8ce 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 18:3493de6fe8ce 617 pc.printf("3 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 618 //Ph23();
simontruelove 18:3493de6fe8ce 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 18:3493de6fe8ce 623 pc.printf("2 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 624 //Ph12();
simontruelove 18:3493de6fe8ce 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 18:3493de6fe8ce 629 pc.printf("1 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 18:3493de6fe8ce 630 //Ph41();
simontruelove 18:3493de6fe8ce 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 17:19b2c598810a 744 if(c == '1')
simontruelove 17:19b2c598810a 745 {
simontruelove 17:19b2c598810a 746 Ph1();
simontruelove 17:19b2c598810a 747 }
simontruelove 17:19b2c598810a 748 if(c == '2')
simontruelove 17:19b2c598810a 749 {
simontruelove 17:19b2c598810a 750 Ph2();
simontruelove 17:19b2c598810a 751 }
simontruelove 17:19b2c598810a 752 if(c == '3')
simontruelove 17:19b2c598810a 753 {
simontruelove 17:19b2c598810a 754 Ph3();
simontruelove 17:19b2c598810a 755 }
simontruelove 17:19b2c598810a 756 if(c == '4')
simontruelove 17:19b2c598810a 757 {
simontruelove 17:19b2c598810a 758 Ph4();
simontruelove 17:19b2c598810a 759 }
simontruelove 17:19b2c598810a 760 if(c == '5')
simontruelove 17:19b2c598810a 761 {
simontruelove 17:19b2c598810a 762 Ph0();
simontruelove 17:19b2c598810a 763 }
simontruelove 17:19b2c598810a 764 if(c == '6')
simontruelove 17:19b2c598810a 765 {
simontruelove 17:19b2c598810a 766 StepACW();
simontruelove 17:19b2c598810a 767 }
simontruelove 18:3493de6fe8ce 768 if(c == '7')
simontruelove 18:3493de6fe8ce 769 {
simontruelove 18:3493de6fe8ce 770 pc.printf("TimePerClick = %i, TimePerRev = %.5f, RPS = %.5f, rpm = %.5f, \n\r", TimePerClick, TimePerRev, RPS, rpm);
simontruelove 18:3493de6fe8ce 771 }
simontruelove 10:808cb9052f14 772 if(c == 'z') //turn on led1 causes CW operation
simontruelove 4:3aedc9246ae4 773 {
simontruelove 4:3aedc9246ae4 774 led1 = !led1;
simontruelove 4:3aedc9246ae4 775 led2 = 0;
simontruelove 13:da9d3fbbe407 776 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 777 }
simontruelove 10:808cb9052f14 778 if(c == 'x') //turn on led2 causes ACW operation
simontruelove 4:3aedc9246ae4 779 {
simontruelove 4:3aedc9246ae4 780 led1 = 0;
simontruelove 4:3aedc9246ae4 781 led2 = !led2 ;
simontruelove 10:808cb9052f14 782 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 783 }
simontruelove 10:808cb9052f14 784 if(c == 'q') //Increases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 785 {
simontruelove 13:da9d3fbbe407 786 SetPoint=SetPoint+5;
simontruelove 14:1eb49362a607 787 if (SetPoint >3000)
simontruelove 10:808cb9052f14 788 {
simontruelove 14:1eb49362a607 789 SetPoint = 3000;
simontruelove 10:808cb9052f14 790 }
simontruelove 10:808cb9052f14 791 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 7:b8de1529c7fc 792 }
simontruelove 10:808cb9052f14 793 if(c == 'a') //Decreases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 794 {
simontruelove 13:da9d3fbbe407 795 SetPoint=SetPoint-5;
simontruelove 17:19b2c598810a 796 if (SetPoint <0)
simontruelove 9:061600a6c750 797 {
simontruelove 17:19b2c598810a 798 SetPoint = 0;
simontruelove 9:061600a6c750 799 }
simontruelove 10:808cb9052f14 800 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 801 }
simontruelove 14:1eb49362a607 802 if(c == 'w') //Increases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 803 {
simontruelove 16:e6c8df9960c6 804 duty = duty + 0.01;
simontruelove 16:e6c8df9960c6 805 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 806 }
simontruelove 14:1eb49362a607 807 if(c == 's') //Decreases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 808 {
simontruelove 16:e6c8df9960c6 809 duty = duty - 0.01;
simontruelove 16:e6c8df9960c6 810 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 811 }
simontruelove 14:1eb49362a607 812 if(c == 'e') //Increases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 813 {
simontruelove 14:1eb49362a607 814 AdjDiff = AdjDiff * 10;
simontruelove 16:e6c8df9960c6 815 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 816 }
simontruelove 14:1eb49362a607 817 if(c == 'd') //Decreases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 818 {
simontruelove 14:1eb49362a607 819 AdjDiff = AdjDiff / 10;
simontruelove 16:e6c8df9960c6 820 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 821 }
simontruelove 10:808cb9052f14 822 if(c== 'o')
simontruelove 10:808cb9052f14 823 {
simontruelove 10:808cb9052f14 824 AdjCW = AdjCW+1;
simontruelove 14:1eb49362a607 825 if (AdjCW >(s-1))
simontruelove 10:808cb9052f14 826 {
simontruelove 10:808cb9052f14 827 AdjCW = 0;
simontruelove 10:808cb9052f14 828 }
simontruelove 10:808cb9052f14 829 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 830 }
simontruelove 10:808cb9052f14 831 if(c== 'k')
simontruelove 10:808cb9052f14 832 {
simontruelove 10:808cb9052f14 833 AdjCW = AdjCW-1;
simontruelove 10:808cb9052f14 834 if (AdjCW <0)
simontruelove 10:808cb9052f14 835 {
simontruelove 14:1eb49362a607 836 AdjCW = (s-1);
simontruelove 10:808cb9052f14 837 }
simontruelove 10:808cb9052f14 838 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 839 }
simontruelove 10:808cb9052f14 840 if(c== 'p')
simontruelove 10:808cb9052f14 841 {
simontruelove 10:808cb9052f14 842 AdjACW = AdjACW+1;
simontruelove 14:1eb49362a607 843 if (AdjACW >(s-1))
simontruelove 10:808cb9052f14 844 {
simontruelove 10:808cb9052f14 845 AdjACW = 0;
simontruelove 10:808cb9052f14 846 }
simontruelove 10:808cb9052f14 847 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 848 }
simontruelove 10:808cb9052f14 849 if(c== 'l')
simontruelove 10:808cb9052f14 850 {
simontruelove 10:808cb9052f14 851 AdjACW = AdjACW-1;
simontruelove 10:808cb9052f14 852 if (AdjACW <0)
simontruelove 10:808cb9052f14 853 {
simontruelove 14:1eb49362a607 854 AdjACW = (s-1);
simontruelove 10:808cb9052f14 855 }
simontruelove 10:808cb9052f14 856 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 857 }
simontruelove 16:e6c8df9960c6 858 if(c== 'i')
simontruelove 16:e6c8df9960c6 859 {
simontruelove 16:e6c8df9960c6 860 n = n+1;
simontruelove 16:e6c8df9960c6 861 /*if (n > 63)
simontruelove 16:e6c8df9960c6 862 {
simontruelove 16:e6c8df9960c6 863 n = 0;
simontruelove 16:e6c8df9960c6 864 }*/
simontruelove 16:e6c8df9960c6 865 pc.printf("%i, %.5f, %i, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n);
simontruelove 16:e6c8df9960c6 866 }
simontruelove 16:e6c8df9960c6 867 if(c== 'j')
simontruelove 16:e6c8df9960c6 868 {
simontruelove 16:e6c8df9960c6 869 n = n-1;
simontruelove 16:e6c8df9960c6 870 /*if (n <0)
simontruelove 16:e6c8df9960c6 871 {
simontruelove 16:e6c8df9960c6 872 n = 63;
simontruelove 16:e6c8df9960c6 873 }*/
simontruelove 16:e6c8df9960c6 874 pc.printf("%i, %.5f, %i, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n);
simontruelove 16:e6c8df9960c6 875 }
simontruelove 16:e6c8df9960c6 876 if(c== 'r')
simontruelove 16:e6c8df9960c6 877 {
simontruelove 16:e6c8df9960c6 878 p = p+0.0000001;
simontruelove 16:e6c8df9960c6 879 pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);
simontruelove 16:e6c8df9960c6 880 }
simontruelove 16:e6c8df9960c6 881 if(c== 'f')
simontruelove 16:e6c8df9960c6 882 {
simontruelove 16:e6c8df9960c6 883 p = p-0.0000001;
simontruelove 16:e6c8df9960c6 884 pc.printf("%i, %.5f, %i, %i, %i, %0.7f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, p);
simontruelove 16:e6c8df9960c6 885 }
simontruelove 10:808cb9052f14 886 if(c=='0')
simontruelove 10:808cb9052f14 887 {
simontruelove 16:e6c8df9960c6 888 pc.printf("%i, %.5f, %i, %i, %i, %i, %f, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, n, AdjDiff, p);
simontruelove 8:2e690f407ec6 889 }
simontruelove 11:74eeb8871fe6 890 if(c=='t')
simontruelove 11:74eeb8871fe6 891 {
simontruelove 12:cbea987a3ec4 892 pc.printf("%.0f C\n\r",temp);
simontruelove 11:74eeb8871fe6 893 }
simontruelove 14:1eb49362a607 894 if(c=='y')
simontruelove 14:1eb49362a607 895 {
simontruelove 16:e6c8df9960c6 896 pc.printf("State A = %i, StateB = %i, Pulses= %i, Revs= %i \n\r", StateA, StateB, wheel.getPulses(), wheel.getRevolutions());
simontruelove 14:1eb49362a607 897 }
simontruelove 4:3aedc9246ae4 898 }
simontruelove 5:4e5c644d5cc3 899 }
simontruelove 5:4e5c644d5cc3 900
simontruelove 5:4e5c644d5cc3 901 void RPM (void)
simontruelove 5:4e5c644d5cc3 902 {
simontruelove 17:19b2c598810a 903 wheel.ResetWhoop(); //Whoop = 1 x per rev
simontruelove 14:1eb49362a607 904 TimePerClick = (t.read_us()); //read timer in microseconds
simontruelove 14:1eb49362a607 905 t.reset(); //reset timer
simontruelove 17:19b2c598810a 906 TimePerRev = TimePerClick; //enc * TimePerClick / z; //enc = 3200, z = 3200 (PulseCount2_==1600)
simontruelove 14:1eb49362a607 907 TimePerRev = TimePerRev / 1000000; // 1microsecond = 0.000001s
simontruelove 14:1eb49362a607 908 RPS = 1 / TimePerRev; //inverse to convert SPR to RPS
simontruelove 14:1eb49362a607 909 rpm = RPS * 60; //x 60 to convert RPS to RPM
simontruelove 14:1eb49362a607 910 //Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
simontruelove 14:1eb49362a607 911 //ReadKType();
simontruelove 16:e6c8df9960c6 912 if(duty < 0.7)
simontruelove 14:1eb49362a607 913 {
simontruelove 16:e6c8df9960c6 914 if(rpm < 65)
simontruelove 16:e6c8df9960c6 915 {
simontruelove 16:e6c8df9960c6 916 AdjCW = (57);
simontruelove 17:19b2c598810a 917 AdjACW = (14);
simontruelove 16:e6c8df9960c6 918 }
simontruelove 16:e6c8df9960c6 919 if(rpm > 64 and rpm <75)
simontruelove 16:e6c8df9960c6 920 {
simontruelove 16:e6c8df9960c6 921 AdjCW = (57);
simontruelove 16:e6c8df9960c6 922 AdjACW = (12);
simontruelove 16:e6c8df9960c6 923 }
simontruelove 16:e6c8df9960c6 924 if(rpm > 74 and rpm < 90)
simontruelove 16:e6c8df9960c6 925 {
simontruelove 16:e6c8df9960c6 926 AdjCW = (60);
simontruelove 16:e6c8df9960c6 927 AdjACW = (9);
simontruelove 16:e6c8df9960c6 928 }
simontruelove 16:e6c8df9960c6 929 if(rpm > 89 and rpm < 200)
simontruelove 16:e6c8df9960c6 930 {
simontruelove 16:e6c8df9960c6 931 AdjCW = (60);
simontruelove 16:e6c8df9960c6 932 AdjACW = (9);
simontruelove 16:e6c8df9960c6 933 }
simontruelove 16:e6c8df9960c6 934 if(rpm > 199 and rpm < 300)
simontruelove 16:e6c8df9960c6 935 {
simontruelove 16:e6c8df9960c6 936 AdjCW = (61);
simontruelove 16:e6c8df9960c6 937 AdjACW = (8);
simontruelove 16:e6c8df9960c6 938 }
simontruelove 16:e6c8df9960c6 939 if(rpm > 299 and rpm <400)
simontruelove 16:e6c8df9960c6 940 {
simontruelove 16:e6c8df9960c6 941 AdjCW = (62);
simontruelove 16:e6c8df9960c6 942 AdjACW = (7);
simontruelove 16:e6c8df9960c6 943 }
simontruelove 16:e6c8df9960c6 944 if(rpm > 399 and rpm < 525)
simontruelove 16:e6c8df9960c6 945 {
simontruelove 16:e6c8df9960c6 946 AdjCW = (63);
simontruelove 16:e6c8df9960c6 947 AdjACW = (6);
simontruelove 16:e6c8df9960c6 948 }
simontruelove 16:e6c8df9960c6 949 if(rpm > 524 and rpm < 625)
simontruelove 16:e6c8df9960c6 950 {
simontruelove 16:e6c8df9960c6 951 AdjCW = (0);
simontruelove 16:e6c8df9960c6 952 AdjACW = (5);
simontruelove 16:e6c8df9960c6 953 }
simontruelove 16:e6c8df9960c6 954 if(rpm > 624 and rpm < 725)
simontruelove 16:e6c8df9960c6 955 {
simontruelove 16:e6c8df9960c6 956 AdjCW = (1);
simontruelove 16:e6c8df9960c6 957 AdjACW = (4);
simontruelove 16:e6c8df9960c6 958 }
simontruelove 16:e6c8df9960c6 959 if(rpm > 724 and rpm < 850)
simontruelove 16:e6c8df9960c6 960 {
simontruelove 16:e6c8df9960c6 961 AdjCW = (2);
simontruelove 16:e6c8df9960c6 962 AdjACW = (3);
simontruelove 16:e6c8df9960c6 963 }
simontruelove 16:e6c8df9960c6 964 if(rpm > 849 and rpm < 975)
simontruelove 16:e6c8df9960c6 965 {
simontruelove 16:e6c8df9960c6 966 AdjCW = (3);
simontruelove 16:e6c8df9960c6 967 AdjACW = (2);
simontruelove 16:e6c8df9960c6 968 }
simontruelove 16:e6c8df9960c6 969 if(rpm > 974 and rpm < 1150)
simontruelove 16:e6c8df9960c6 970 {
simontruelove 16:e6c8df9960c6 971 AdjCW = (4);
simontruelove 17:19b2c598810a 972 AdjACW = (1);
simontruelove 16:e6c8df9960c6 973 }
simontruelove 16:e6c8df9960c6 974 if(rpm > 1149 and rpm < 1375)
simontruelove 16:e6c8df9960c6 975 {
simontruelove 16:e6c8df9960c6 976 AdjCW = (5);
simontruelove 16:e6c8df9960c6 977 AdjACW = (0);
simontruelove 16:e6c8df9960c6 978 }
simontruelove 16:e6c8df9960c6 979 if(rpm > 1374 and rpm < 1725)
simontruelove 16:e6c8df9960c6 980 {
simontruelove 16:e6c8df9960c6 981 AdjCW = (6);
simontruelove 16:e6c8df9960c6 982 AdjACW = (63);
simontruelove 16:e6c8df9960c6 983 }
simontruelove 16:e6c8df9960c6 984 if(rpm > 1724 and rpm < 2150)
simontruelove 16:e6c8df9960c6 985 {
simontruelove 16:e6c8df9960c6 986 AdjCW = (7);
simontruelove 16:e6c8df9960c6 987 AdjACW = (62);
simontruelove 16:e6c8df9960c6 988 }
simontruelove 16:e6c8df9960c6 989 if(rpm > 2149 and rpm < 2525)
simontruelove 16:e6c8df9960c6 990 {
simontruelove 16:e6c8df9960c6 991 AdjCW = (8);
simontruelove 16:e6c8df9960c6 992 AdjACW = (61);
simontruelove 16:e6c8df9960c6 993 }
simontruelove 16:e6c8df9960c6 994 if(rpm > 2524 and rpm < 2600)
simontruelove 16:e6c8df9960c6 995 {
simontruelove 16:e6c8df9960c6 996 AdjCW = (9);
simontruelove 16:e6c8df9960c6 997 AdjACW = (60);
simontruelove 16:e6c8df9960c6 998 }
simontruelove 16:e6c8df9960c6 999 if(rpm > 2599 and rpm < 2650)
simontruelove 16:e6c8df9960c6 1000 {
simontruelove 16:e6c8df9960c6 1001 AdjCW = (10);
simontruelove 16:e6c8df9960c6 1002 AdjACW = (59);
simontruelove 16:e6c8df9960c6 1003 }
simontruelove 16:e6c8df9960c6 1004 if(rpm > 2649 and rpm < 2700)
simontruelove 16:e6c8df9960c6 1005 {
simontruelove 16:e6c8df9960c6 1006 AdjCW = (11);
simontruelove 16:e6c8df9960c6 1007 AdjACW = (58);
simontruelove 16:e6c8df9960c6 1008 }
simontruelove 16:e6c8df9960c6 1009 if(rpm > 2699 and rpm < 2750)
simontruelove 16:e6c8df9960c6 1010 {
simontruelove 16:e6c8df9960c6 1011 AdjCW = (12);
simontruelove 16:e6c8df9960c6 1012 AdjACW = (57);
simontruelove 16:e6c8df9960c6 1013 }
simontruelove 16:e6c8df9960c6 1014 if(rpm > 2749 and rpm < 2825)
simontruelove 16:e6c8df9960c6 1015 {
simontruelove 16:e6c8df9960c6 1016 AdjCW = (13);
simontruelove 16:e6c8df9960c6 1017 AdjACW = (56);
simontruelove 16:e6c8df9960c6 1018 }
simontruelove 16:e6c8df9960c6 1019 if(rpm > 2824 and rpm < 2875)
simontruelove 16:e6c8df9960c6 1020 {
simontruelove 16:e6c8df9960c6 1021 AdjCW = (14);
simontruelove 16:e6c8df9960c6 1022 AdjACW = (55);
simontruelove 16:e6c8df9960c6 1023 }
simontruelove 16:e6c8df9960c6 1024 if(rpm > 2874)
simontruelove 16:e6c8df9960c6 1025 {
simontruelove 16:e6c8df9960c6 1026 AdjCW = (15);
simontruelove 16:e6c8df9960c6 1027 AdjACW = (54);
simontruelove 16:e6c8df9960c6 1028 }
simontruelove 14:1eb49362a607 1029 }
simontruelove 16:e6c8df9960c6 1030 if(duty > 0.69)
simontruelove 14:1eb49362a607 1031 {
simontruelove 16:e6c8df9960c6 1032 if(rpm < 175)
simontruelove 16:e6c8df9960c6 1033 {
simontruelove 16:e6c8df9960c6 1034 AdjCW = (60);
simontruelove 17:19b2c598810a 1035 AdjACW = (14);
simontruelove 16:e6c8df9960c6 1036 }
simontruelove 17:19b2c598810a 1037 if(rpm > 200 and rpm < 1000)
simontruelove 16:e6c8df9960c6 1038 {
simontruelove 16:e6c8df9960c6 1039 AdjCW = (5);
simontruelove 16:e6c8df9960c6 1040 AdjACW = (5);
simontruelove 16:e6c8df9960c6 1041 }
simontruelove 14:1eb49362a607 1042 }
simontruelove 14:1eb49362a607 1043 //pc.printf("RPS = %i \r", RPS);
simontruelove 6:f7028034aabb 1044 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 7:b8de1529c7fc 1045 }
simontruelove 7:b8de1529c7fc 1046
simontruelove 7:b8de1529c7fc 1047 void VelocityLoop (void)
simontruelove 7:b8de1529c7fc 1048 {
simontruelove 18:3493de6fe8ce 1049 diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
simontruelove 18:3493de6fe8ce 1050 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 1051 /*if (duty < 0.8)
simontruelove 14:1eb49362a607 1052 {
simontruelove 16:e6c8df9960c6 1053 j = 0;
simontruelove 16:e6c8df9960c6 1054 k = 0;
simontruelove 16:e6c8df9960c6 1055 l = 0;
simontruelove 16:e6c8df9960c6 1056 m = 0;
simontruelove 16:e6c8df9960c6 1057 n = 0;
simontruelove 14:1eb49362a607 1058 }
simontruelove 16:e6c8df9960c6 1059 if (duty > 0.79)
simontruelove 14:1eb49362a607 1060 {
simontruelove 16:e6c8df9960c6 1061 i = n-5;
simontruelove 16:e6c8df9960c6 1062 j = n-4;
simontruelove 16:e6c8df9960c6 1063 k = n-3;
simontruelove 16:e6c8df9960c6 1064 l = n-2;
simontruelove 16:e6c8df9960c6 1065 m = n-1;
simontruelove 16:e6c8df9960c6 1066 n = 6;
simontruelove 14:1eb49362a607 1067 }*/
simontruelove 14:1eb49362a607 1068 if (duty > 1) //limits for duty. Motor will not operate below 0.96. 1 = max
simontruelove 14:1eb49362a607 1069 {
simontruelove 9:061600a6c750 1070 duty = 1;
simontruelove 7:b8de1529c7fc 1071 }
simontruelove 16:e6c8df9960c6 1072 if (duty <0.08) //3A min duty 0.96, 6.5A min duty 0.4
simontruelove 10:808cb9052f14 1073 {
simontruelove 16:e6c8df9960c6 1074 duty = 0.08;
simontruelove 7:b8de1529c7fc 1075 }
simontruelove 10:808cb9052f14 1076 //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 1077 }
simontruelove 11:74eeb8871fe6 1078
simontruelove 11:74eeb8871fe6 1079 void ReadKType(void)
simontruelove 11:74eeb8871fe6 1080 {
simontruelove 11:74eeb8871fe6 1081 int i = 0;
simontruelove 11:74eeb8871fe6 1082 int Readout = 0;
simontruelove 11:74eeb8871fe6 1083
simontruelove 11:74eeb8871fe6 1084 cs1 = 0;
simontruelove 14:1eb49362a607 1085 SerialClock = 0; //set clock to 0
simontruelove 16:e6c8df9960c6 1086 wait_ms(0.01);
simontruelove 14:1eb49362a607 1087 SerialClock = 1; //clock once to set to the 13 bit temp data
simontruelove 16:e6c8df9960c6 1088 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1089 SerialClock = 0;
simontruelove 16:e6c8df9960c6 1090 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1091
simontruelove 11:74eeb8871fe6 1092 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 1093 {
simontruelove 11:74eeb8871fe6 1094 if(DOut == 1) //check data, store results in readout
simontruelove 11:74eeb8871fe6 1095 {
simontruelove 12:cbea987a3ec4 1096 Readout |= (1<<i);
simontruelove 11:74eeb8871fe6 1097 }
simontruelove 11:74eeb8871fe6 1098 else
simontruelove 11:74eeb8871fe6 1099 {
simontruelove 11:74eeb8871fe6 1100 Readout |= (0<<i);
simontruelove 11:74eeb8871fe6 1101 }
simontruelove 12:cbea987a3ec4 1102
simontruelove 11:74eeb8871fe6 1103 SerialClock = 1; //clock to the next bit
simontruelove 16:e6c8df9960c6 1104 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1105 SerialClock = 0;
simontruelove 16:e6c8df9960c6 1106 wait_ms(0.01);
simontruelove 11:74eeb8871fe6 1107 }
simontruelove 11:74eeb8871fe6 1108 temp = Readout * 0.125; //get the real temp value which is a float
simontruelove 16:e6c8df9960c6 1109 //pc.printf("%.1f\r",temp);
simontruelove 16:e6c8df9960c6 1110 //pc.printf("%i, %.4f\n\r", rpm, duty);
simontruelove 11:74eeb8871fe6 1111 Readout = 0;
simontruelove 11:74eeb8871fe6 1112 cs1 = 1;
simontruelove 3:4249dbdf7ed3 1113 }