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@14:1eb49362a607, 2019-01-25 (annotated)

Committer:
simontruelove
Date:
Fri Jan 25 15:04:56 2019 +0000
Revision:
14:1eb49362a607
Parent:
13:da9d3fbbe407
Child:
15:12a4bbfa6de4
Fully working unloaded 64 state machine SRM from 0 - 3000 rpm. Auto phase advance and velocity control

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 1:0191658b6ff4 5 void StepCW(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 14:1eb49362a607 28 PwmOut Phase3 (p25);
simontruelove 14:1eb49362a607 29 PwmOut Phase4 (p26);
simontruelove 14:1eb49362a607 30 DigitalOut UnUsedPhase1 (p23);
simontruelove 14:1eb49362a607 31 DigitalOut UnUsedPhase2 (p24);
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 14:1eb49362a607 58 int s = enc/50;
simontruelove 14:1eb49362a607 59 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 60 int T = 80; //Motor temp limit
simontruelove 14:1eb49362a607 61 int slowloop = 0;
simontruelove 11:74eeb8871fe6 62
simontruelove 10:808cb9052f14 63 char c; //keyboard cotrol GetChar
simontruelove 4:3aedc9246ae4 64
simontruelove 13:da9d3fbbe407 65 float duty = 1; //velocity loop: 1 = fastest, 0.96 = slowest. Below 0.96 the motor will not operate.
simontruelove 10:808cb9052f14 66 float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
simontruelove 14:1eb49362a607 67 float AdjDiff = 0.0001;
simontruelove 10:808cb9052f14 68 float x=0.1; //x=time of square wave when 1 phase energised,
simontruelove 14:1eb49362a607 69 float TimePerRev = 0; //for calc of RPM
simontruelove 10:808cb9052f14 70 float y=0.04; //y=time of square wave when 2 phases energised
simontruelove 10:808cb9052f14 71 float temp = 0; //ReadKType
simontruelove 1:0191658b6ff4 72
simontruelove 1:0191658b6ff4 73 int main(void)
simontruelove 3:4249dbdf7ed3 74 {
simontruelove 10:808cb9052f14 75 pc.baud(230400); //Set fastest baud rate
simontruelove 14:1eb49362a607 76 Phase1.period(0.00001); //period of 0.000002 = 2 microseconds (500kHz). Good balance of low and high speed performance.
simontruelove 14:1eb49362a607 77 Phase2.period(0.00001);
simontruelove 14:1eb49362a607 78 Phase3.period(0.00001);
simontruelove 14:1eb49362a607 79 Phase4.period(0.00001);
simontruelove 14:1eb49362a607 80 wait(0.1);
simontruelove 14:1eb49362a607 81 t.start();
simontruelove 14:1eb49362a607 82 SerialClock = 0;
simontruelove 3:4249dbdf7ed3 83 StepCW();
simontruelove 3:4249dbdf7ed3 84 Initialisation();
simontruelove 14:1eb49362a607 85
simontruelove 14:1eb49362a607 86 /*while(1)
simontruelove 14:1eb49362a607 87 {
simontruelove 14:1eb49362a607 88
simontruelove 14:1eb49362a607 89 }
simontruelove 14:1eb49362a607 90 */
simontruelove 14:1eb49362a607 91
simontruelove 10:808cb9052f14 92 while(wheel.getRevolutions()<2) //Index Calibration
simontruelove 14:1eb49362a607 93 {
simontruelove 14:1eb49362a607 94 StateA = (wheel.getPulses()+25)%s;
simontruelove 5:4e5c644d5cc3 95 switch(StateA)
simontruelove 3:4249dbdf7ed3 96 {
simontruelove 14:1eb49362a607 97 case 0:Ph1();break;//;pc.printf("1 Pulses= %i\n\r", wheel.getPulses());break;
simontruelove 6:f7028034aabb 98 case 1:Ph1();break;
simontruelove 14:1eb49362a607 99 case 2:Ph1();break;
simontruelove 14:1eb49362a607 100 case 3:Ph1();break;
simontruelove 14:1eb49362a607 101 case 4:Ph1();break;
simontruelove 14:1eb49362a607 102 case 5:Ph1();break;
simontruelove 14:1eb49362a607 103 case 6:Ph1();break;
simontruelove 14:1eb49362a607 104 case 7:Ph1();break;
simontruelove 14:1eb49362a607 105 case 8:Ph1();break;
simontruelove 14:1eb49362a607 106 case 9:Ph1();break;
simontruelove 14:1eb49362a607 107 case 10:Ph1();break;
simontruelove 14:1eb49362a607 108 case 11:Ph1();break;
simontruelove 14:1eb49362a607 109 case 12:Ph1();break;
simontruelove 14:1eb49362a607 110 case 13:Ph1();break;
simontruelove 14:1eb49362a607 111 case 14:Ph1();break;
simontruelove 14:1eb49362a607 112 case 15:Ph1();break;
simontruelove 14:1eb49362a607 113 case 16:Ph2();break;//;pc.printf("2 Pulses= %i\n\r", wheel.getPulses());break;
simontruelove 14:1eb49362a607 114 case 17:Ph2();break;
simontruelove 14:1eb49362a607 115 case 18:Ph2();break;
simontruelove 14:1eb49362a607 116 case 19:Ph2();break;
simontruelove 14:1eb49362a607 117 case 20:Ph2();break;
simontruelove 14:1eb49362a607 118 case 21:Ph2();break;
simontruelove 14:1eb49362a607 119 case 22:Ph2();break;
simontruelove 14:1eb49362a607 120 case 23:Ph2();break;
simontruelove 14:1eb49362a607 121 case 24:Ph2();break;
simontruelove 14:1eb49362a607 122 case 25:Ph2();break;
simontruelove 14:1eb49362a607 123 case 26:Ph2();break;
simontruelove 14:1eb49362a607 124 case 27:Ph2();break;
simontruelove 14:1eb49362a607 125 case 28:Ph2();break;
simontruelove 14:1eb49362a607 126 case 29:Ph2();break;
simontruelove 14:1eb49362a607 127 case 30:Ph2();break;
simontruelove 14:1eb49362a607 128 case 31:Ph2();break;
simontruelove 14:1eb49362a607 129 case 32:Ph3();break;//;pc.printf("3 Pulses= %i\n\r", wheel.getPulses());break;
simontruelove 14:1eb49362a607 130 case 33:Ph3();break;
simontruelove 14:1eb49362a607 131 case 34:Ph3();break;
simontruelove 14:1eb49362a607 132 case 35:Ph3();break;
simontruelove 14:1eb49362a607 133 case 36:Ph3();break;
simontruelove 14:1eb49362a607 134 case 37:Ph3();break;
simontruelove 14:1eb49362a607 135 case 38:Ph3();break;
simontruelove 14:1eb49362a607 136 case 39:Ph3();break;
simontruelove 14:1eb49362a607 137 case 40:Ph3();break;
simontruelove 14:1eb49362a607 138 case 41:Ph3();break;
simontruelove 14:1eb49362a607 139 case 42:Ph3();break;
simontruelove 14:1eb49362a607 140 case 43:Ph3();break;
simontruelove 14:1eb49362a607 141 case 44:Ph3();break;
simontruelove 14:1eb49362a607 142 case 45:Ph3();break;
simontruelove 14:1eb49362a607 143 case 46:Ph3();break;
simontruelove 14:1eb49362a607 144 case 47:Ph3();break;
simontruelove 14:1eb49362a607 145 case 48:Ph4();break;//;pc.printf("4 Pulses= %i\n\r", wheel.getPulses());break;
simontruelove 14:1eb49362a607 146 case 49:Ph4();break;
simontruelove 14:1eb49362a607 147 case 50:Ph4();break;
simontruelove 14:1eb49362a607 148 case 51:Ph4();break;
simontruelove 14:1eb49362a607 149 case 52:Ph4();break;
simontruelove 14:1eb49362a607 150 case 53:Ph4();break;
simontruelove 14:1eb49362a607 151 case 54:Ph4();break;
simontruelove 14:1eb49362a607 152 case 55:Ph4();break;
simontruelove 14:1eb49362a607 153 case 56:Ph4();break;
simontruelove 14:1eb49362a607 154 case 57:Ph4();break;
simontruelove 14:1eb49362a607 155 case 58:Ph4();break;
simontruelove 14:1eb49362a607 156 case 59:Ph4();break;
simontruelove 14:1eb49362a607 157 case 60:Ph4();break;
simontruelove 14:1eb49362a607 158 case 61:Ph4();break;
simontruelove 14:1eb49362a607 159 case 62:Ph4();break;
simontruelove 14:1eb49362a607 160 case 63:Ph4();break;
simontruelove 5:4e5c644d5cc3 161 default:break;
simontruelove 3:4249dbdf7ed3 162 }
simontruelove 10:808cb9052f14 163 if(wheel.getYay()==1) //PulseCount_==1, yay_=1;
simontruelove 3:4249dbdf7ed3 164 {
simontruelove 5:4e5c644d5cc3 165 StateA++;
simontruelove 5:4e5c644d5cc3 166 wheel.ResetYay();
simontruelove 14:1eb49362a607 167 if (StateA>(s-1))
simontruelove 3:4249dbdf7ed3 168 {
simontruelove 5:4e5c644d5cc3 169 StateA=0;
simontruelove 3:4249dbdf7ed3 170 }
simontruelove 3:4249dbdf7ed3 171 }
simontruelove 14:1eb49362a607 172 }
simontruelove 3:4249dbdf7ed3 173
simontruelove 5:4e5c644d5cc3 174 while(1)
simontruelove 5:4e5c644d5cc3 175 {
simontruelove 10:808cb9052f14 176 while((led1 == 0) && (led2 == 0)) //If no command to operate
simontruelove 4:3aedc9246ae4 177 {
simontruelove 12:cbea987a3ec4 178 //Aout = 0;
simontruelove 14:1eb49362a607 179 //duty = 0.5;
simontruelove 10:808cb9052f14 180 rpm = 0;
simontruelove 14:1eb49362a607 181 AdjCW = CW;
simontruelove 14:1eb49362a607 182 AdjACW = ACW;
simontruelove 14:1eb49362a607 183 Ph0();
simontruelove 10:808cb9052f14 184 GetChar();
simontruelove 11:74eeb8871fe6 185 ReadKType();
simontruelove 14:1eb49362a607 186 StateB = (wheel.getPulses()+StateA)%s; //wheel.getPulses()%1(s-1);
simontruelove 14:1eb49362a607 187 //StateC = (800+wheel.getPulses()+StateA+AdjCW)%s;
simontruelove 14:1eb49362a607 188 //if(wheel.getPulses()==wheel.getPulses()+1);
simontruelove 14:1eb49362a607 189 //{
simontruelove 14:1eb49362a607 190 //pc.printf("B StateA= %i, Pulses= %i, Revs= %i\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 191 //pc.printf("StateA= %i, StateB= %i, Pulses = %i \r", StateA, StateB, wheel.getPulses());
simontruelove 14:1eb49362a607 192 //}
simontruelove 5:4e5c644d5cc3 193 //pc.printf("0 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 194 }
simontruelove 11:74eeb8871fe6 195 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
simontruelove 3:4249dbdf7ed3 196 {
simontruelove 4:3aedc9246ae4 197 GetChar();
simontruelove 14:1eb49362a607 198 StateB = (wheel.getPulses()+StateA+AdjCW)%s;
simontruelove 14:1eb49362a607 199 //pc.printf("rpm = %i, whoop = %i\n\r", rpm, wheel.getWhoop());
simontruelove 6:f7028034aabb 200 //pc.printf("StateB= %i\n\r", StateB);
simontruelove 14:1eb49362a607 201 //pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 202
simontruelove 4:3aedc9246ae4 203 switch(StateB)
simontruelove 4:3aedc9246ae4 204 {
simontruelove 6:f7028034aabb 205 case 0:Ph1();break;
simontruelove 6:f7028034aabb 206 case 1:Ph1();break;
simontruelove 13:da9d3fbbe407 207 case 2:Ph1();break;
simontruelove 13:da9d3fbbe407 208 case 3:Ph1();break;
simontruelove 14:1eb49362a607 209 case 4:Ph1();break;
simontruelove 14:1eb49362a607 210 case 5:Ph1();break;
simontruelove 14:1eb49362a607 211 case 6:Ph1();break;
simontruelove 14:1eb49362a607 212 case 7:Ph1();break;
simontruelove 14:1eb49362a607 213 case 8:Ph1();break;
simontruelove 14:1eb49362a607 214 case 9:Ph1();break;
simontruelove 14:1eb49362a607 215 case 10:Ph1();break;
simontruelove 14:1eb49362a607 216 case 11:Ph1();break;
simontruelove 14:1eb49362a607 217 case 12:Ph1();break;
simontruelove 14:1eb49362a607 218 case 13:Ph1();break;
simontruelove 14:1eb49362a607 219 case 14:Ph1();break;
simontruelove 14:1eb49362a607 220 case 15:Ph1();break;
simontruelove 14:1eb49362a607 221 case 16:Ph2();break;
simontruelove 14:1eb49362a607 222 case 17:Ph2();break;
simontruelove 14:1eb49362a607 223 case 18:Ph2();break;
simontruelove 14:1eb49362a607 224 case 19:Ph2();break;
simontruelove 14:1eb49362a607 225 case 20:Ph2();break;
simontruelove 14:1eb49362a607 226 case 21:Ph2();break;
simontruelove 14:1eb49362a607 227 case 22:Ph2();break;
simontruelove 14:1eb49362a607 228 case 23:Ph2();break;
simontruelove 14:1eb49362a607 229 case 24:Ph2();break;
simontruelove 14:1eb49362a607 230 case 25:Ph2();break;
simontruelove 14:1eb49362a607 231 case 26:Ph2();break;
simontruelove 14:1eb49362a607 232 case 27:Ph2();break;
simontruelove 14:1eb49362a607 233 case 28:Ph2();break;
simontruelove 14:1eb49362a607 234 case 29:Ph2();break;
simontruelove 14:1eb49362a607 235 case 30:Ph2();break;
simontruelove 14:1eb49362a607 236 case 31:Ph2();break;
simontruelove 14:1eb49362a607 237 case 32:Ph3();break;
simontruelove 14:1eb49362a607 238 case 33:Ph3();break;
simontruelove 14:1eb49362a607 239 case 34:Ph3();break;
simontruelove 14:1eb49362a607 240 case 35:Ph3();break;
simontruelove 14:1eb49362a607 241 case 36:Ph3();break;
simontruelove 14:1eb49362a607 242 case 37:Ph3();break;
simontruelove 14:1eb49362a607 243 case 38:Ph3();break;
simontruelove 14:1eb49362a607 244 case 39:Ph3();break;
simontruelove 14:1eb49362a607 245 case 40:Ph3();break;
simontruelove 14:1eb49362a607 246 case 41:Ph3();break;
simontruelove 14:1eb49362a607 247 case 42:Ph3();break;
simontruelove 14:1eb49362a607 248 case 43:Ph3();break;
simontruelove 14:1eb49362a607 249 case 44:Ph3();break;
simontruelove 14:1eb49362a607 250 case 45:Ph3();break;
simontruelove 14:1eb49362a607 251 case 46:Ph3();break;
simontruelove 14:1eb49362a607 252 case 47:Ph3();break;
simontruelove 14:1eb49362a607 253 case 48:Ph4();break;
simontruelove 14:1eb49362a607 254 case 49:Ph4();break;
simontruelove 14:1eb49362a607 255 case 50:Ph4();break;
simontruelove 14:1eb49362a607 256 case 51:Ph4();break;
simontruelove 14:1eb49362a607 257 case 52:Ph4();break;
simontruelove 14:1eb49362a607 258 case 53:Ph4();break;
simontruelove 14:1eb49362a607 259 case 54:Ph4();break;
simontruelove 14:1eb49362a607 260 case 55:Ph4();break;
simontruelove 14:1eb49362a607 261 case 56:Ph4();break;
simontruelove 14:1eb49362a607 262 case 57:Ph4();break;
simontruelove 14:1eb49362a607 263 case 58:Ph4();break;
simontruelove 14:1eb49362a607 264 case 59:Ph4();break;
simontruelove 14:1eb49362a607 265 case 60:Ph4();break;
simontruelove 14:1eb49362a607 266 case 61:Ph4();break;
simontruelove 14:1eb49362a607 267 case 62:Ph4();break;
simontruelove 14:1eb49362a607 268 case 63:Ph4();break;
simontruelove 5:4e5c644d5cc3 269 default:break;
simontruelove 4:3aedc9246ae4 270 }
simontruelove 14:1eb49362a607 271 /*if(wheel.getYay()==1)
simontruelove 14:1eb49362a607 272 {
simontruelove 14:1eb49362a607 273 pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 274 }*/
simontruelove 12:cbea987a3ec4 275 if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
simontruelove 4:3aedc9246ae4 276 {
simontruelove 5:4e5c644d5cc3 277 RPM();
simontruelove 9:061600a6c750 278 VelocityLoop();
simontruelove 14:1eb49362a607 279 slowloop++;
simontruelove 14:1eb49362a607 280 if(slowloop>rpm)
simontruelove 14:1eb49362a607 281 {
simontruelove 14:1eb49362a607 282 ReadKType();
simontruelove 14:1eb49362a607 283 slowloop=0;
simontruelove 14:1eb49362a607 284 }
simontruelove 11:74eeb8871fe6 285 }
simontruelove 4:3aedc9246ae4 286 }
simontruelove 4:3aedc9246ae4 287
simontruelove 11:74eeb8871fe6 288 while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
simontruelove 4:3aedc9246ae4 289 {
simontruelove 4:3aedc9246ae4 290 GetChar();
simontruelove 14:1eb49362a607 291 StateB = (enc+wheel.getPulses()+StateA+AdjCW)%s;
simontruelove 6:f7028034aabb 292 //pc.printf("StateA= %i\r", StateA);
simontruelove 14:1eb49362a607 293 //pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 3:4249dbdf7ed3 294
simontruelove 3:4249dbdf7ed3 295 switch(StateB)
simontruelove 3:4249dbdf7ed3 296 {
simontruelove 6:f7028034aabb 297 case 0:Ph1();break;
simontruelove 6:f7028034aabb 298 case 1:Ph1();break;
simontruelove 13:da9d3fbbe407 299 case 2:Ph1();break;
simontruelove 13:da9d3fbbe407 300 case 3:Ph1();break;
simontruelove 14:1eb49362a607 301 case 4:Ph1();break;
simontruelove 14:1eb49362a607 302 case 5:Ph1();break;
simontruelove 14:1eb49362a607 303 case 6:Ph1();break;
simontruelove 14:1eb49362a607 304 case 7:Ph1();break;
simontruelove 14:1eb49362a607 305 case 8:Ph1();break;
simontruelove 14:1eb49362a607 306 case 9:Ph1();break;
simontruelove 14:1eb49362a607 307 case 10:Ph1();break;
simontruelove 14:1eb49362a607 308 case 11:Ph1();break;
simontruelove 14:1eb49362a607 309 case 12:Ph1();break;
simontruelove 14:1eb49362a607 310 case 13:Ph1();break;
simontruelove 14:1eb49362a607 311 case 14:Ph1();break;
simontruelove 14:1eb49362a607 312 case 15:Ph1();break;
simontruelove 14:1eb49362a607 313 case 16:Ph2();break;
simontruelove 14:1eb49362a607 314 case 17:Ph2();break;
simontruelove 14:1eb49362a607 315 case 18:Ph2();break;
simontruelove 14:1eb49362a607 316 case 19:Ph2();break;
simontruelove 14:1eb49362a607 317 case 20:Ph2();break;
simontruelove 14:1eb49362a607 318 case 21:Ph2();break;
simontruelove 14:1eb49362a607 319 case 22:Ph2();break;
simontruelove 14:1eb49362a607 320 case 23:Ph2();break;
simontruelove 14:1eb49362a607 321 case 24:Ph2();break;
simontruelove 14:1eb49362a607 322 case 25:Ph2();break;
simontruelove 14:1eb49362a607 323 case 26:Ph2();break;
simontruelove 14:1eb49362a607 324 case 27:Ph2();break;
simontruelove 14:1eb49362a607 325 case 28:Ph2();break;
simontruelove 14:1eb49362a607 326 case 29:Ph2();break;
simontruelove 14:1eb49362a607 327 case 30:Ph2();break;
simontruelove 14:1eb49362a607 328 case 31:Ph2();break;
simontruelove 14:1eb49362a607 329 case 32:Ph3();break;
simontruelove 14:1eb49362a607 330 case 33:Ph3();break;
simontruelove 14:1eb49362a607 331 case 34:Ph3();break;
simontruelove 14:1eb49362a607 332 case 35:Ph3();break;
simontruelove 14:1eb49362a607 333 case 36:Ph3();break;
simontruelove 14:1eb49362a607 334 case 37:Ph3();break;
simontruelove 14:1eb49362a607 335 case 38:Ph3();break;
simontruelove 14:1eb49362a607 336 case 39:Ph3();break;
simontruelove 14:1eb49362a607 337 case 40:Ph3();break;
simontruelove 14:1eb49362a607 338 case 41:Ph3();break;
simontruelove 14:1eb49362a607 339 case 42:Ph3();break;
simontruelove 14:1eb49362a607 340 case 43:Ph3();break;
simontruelove 14:1eb49362a607 341 case 44:Ph3();break;
simontruelove 14:1eb49362a607 342 case 45:Ph3();break;
simontruelove 14:1eb49362a607 343 case 46:Ph3();break;
simontruelove 14:1eb49362a607 344 case 47:Ph3();break;
simontruelove 14:1eb49362a607 345 case 48:Ph4();break;
simontruelove 14:1eb49362a607 346 case 49:Ph4();break;
simontruelove 14:1eb49362a607 347 case 50:Ph4();break;
simontruelove 14:1eb49362a607 348 case 51:Ph4();break;
simontruelove 14:1eb49362a607 349 case 52:Ph4();break;
simontruelove 14:1eb49362a607 350 case 53:Ph4();break;
simontruelove 14:1eb49362a607 351 case 54:Ph4();break;
simontruelove 14:1eb49362a607 352 case 55:Ph4();break;
simontruelove 14:1eb49362a607 353 case 56:Ph4();break;
simontruelove 14:1eb49362a607 354 case 57:Ph4();break;
simontruelove 14:1eb49362a607 355 case 58:Ph4();break;
simontruelove 14:1eb49362a607 356 case 59:Ph4();break;
simontruelove 14:1eb49362a607 357 case 60:Ph4();break;
simontruelove 14:1eb49362a607 358 case 61:Ph4();break;
simontruelove 14:1eb49362a607 359 case 62:Ph4();break;
simontruelove 14:1eb49362a607 360 case 63:Ph4();break;
simontruelove 5:4e5c644d5cc3 361 default:break;
simontruelove 3:4249dbdf7ed3 362 }
simontruelove 14:1eb49362a607 363 /*if(wheel.getYay()==1)
simontruelove 14:1eb49362a607 364 {
simontruelove 14:1eb49362a607 365 pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 366 }*/
simontruelove 12:cbea987a3ec4 367 if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
simontruelove 5:4e5c644d5cc3 368 {
simontruelove 5:4e5c644d5cc3 369 RPM();
simontruelove 9:061600a6c750 370 VelocityLoop();
simontruelove 14:1eb49362a607 371 slowloop++;
simontruelove 14:1eb49362a607 372 if(slowloop>rpm)
simontruelove 14:1eb49362a607 373 {
simontruelove 14:1eb49362a607 374 ReadKType();
simontruelove 14:1eb49362a607 375 slowloop=0;
simontruelove 14:1eb49362a607 376 }
simontruelove 11:74eeb8871fe6 377 }
simontruelove 5:4e5c644d5cc3 378 }
simontruelove 11:74eeb8871fe6 379 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
simontruelove 4:3aedc9246ae4 380 {
simontruelove 5:4e5c644d5cc3 381 GetChar();
simontruelove 6:f7028034aabb 382 //StateB = (800+wheel.getPulses())%16;
simontruelove 14:1eb49362a607 383 StateB = (wheel.getPulses()+StateA+AdjACW)%s;
simontruelove 6:f7028034aabb 384 //pc.printf("StateA= %i\r", StateA);
simontruelove 14:1eb49362a607 385 //pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 386 switch(StateB)
simontruelove 5:4e5c644d5cc3 387 {
simontruelove 14:1eb49362a607 388 case 63:Ph4();break;
simontruelove 14:1eb49362a607 389 case 62:Ph4();break;
simontruelove 14:1eb49362a607 390 case 61:Ph4();break;
simontruelove 14:1eb49362a607 391 case 60:Ph4();break;
simontruelove 14:1eb49362a607 392 case 59:Ph4();break;
simontruelove 14:1eb49362a607 393 case 58:Ph4();break;
simontruelove 14:1eb49362a607 394 case 57:Ph4();break;
simontruelove 14:1eb49362a607 395 case 56:Ph4();break;
simontruelove 14:1eb49362a607 396 case 55:Ph4();break;
simontruelove 14:1eb49362a607 397 case 54:Ph4();break;
simontruelove 14:1eb49362a607 398 case 53:Ph4();break;
simontruelove 14:1eb49362a607 399 case 52:Ph4();break;
simontruelove 14:1eb49362a607 400 case 51:Ph4();break;
simontruelove 14:1eb49362a607 401 case 50:Ph4();break;
simontruelove 14:1eb49362a607 402 case 49:Ph4();break;
simontruelove 14:1eb49362a607 403 case 48:Ph4();break;
simontruelove 14:1eb49362a607 404 case 47:Ph3();break;
simontruelove 14:1eb49362a607 405 case 46:Ph3();break;
simontruelove 14:1eb49362a607 406 case 45:Ph3();break;
simontruelove 14:1eb49362a607 407 case 44:Ph3();break;
simontruelove 14:1eb49362a607 408 case 43:Ph3();break;
simontruelove 14:1eb49362a607 409 case 42:Ph3();break;
simontruelove 14:1eb49362a607 410 case 41:Ph3();break;
simontruelove 14:1eb49362a607 411 case 40:Ph3();break;
simontruelove 14:1eb49362a607 412 case 39:Ph3();break;
simontruelove 14:1eb49362a607 413 case 38:Ph3();break;
simontruelove 14:1eb49362a607 414 case 37:Ph3();break;
simontruelove 14:1eb49362a607 415 case 36:Ph3();break;
simontruelove 14:1eb49362a607 416 case 35:Ph3();break;
simontruelove 14:1eb49362a607 417 case 34:Ph3();break;
simontruelove 14:1eb49362a607 418 case 33:Ph3();break;
simontruelove 14:1eb49362a607 419 case 32:Ph3();break;
simontruelove 14:1eb49362a607 420 case 31:Ph2();break;
simontruelove 14:1eb49362a607 421 case 30:Ph2();break;
simontruelove 14:1eb49362a607 422 case 29:Ph2();break;
simontruelove 14:1eb49362a607 423 case 28:Ph2();break;
simontruelove 14:1eb49362a607 424 case 27:Ph2();break;
simontruelove 14:1eb49362a607 425 case 26:Ph2();break;
simontruelove 14:1eb49362a607 426 case 25:Ph2();break;
simontruelove 14:1eb49362a607 427 case 24:Ph2();break;
simontruelove 14:1eb49362a607 428 case 23:Ph2();break;
simontruelove 14:1eb49362a607 429 case 22:Ph2();break;
simontruelove 14:1eb49362a607 430 case 21:Ph2();break;
simontruelove 14:1eb49362a607 431 case 20:Ph2();break;
simontruelove 14:1eb49362a607 432 case 19:Ph2();break;
simontruelove 14:1eb49362a607 433 case 18:Ph2();break;
simontruelove 14:1eb49362a607 434 case 17:Ph2();break;
simontruelove 14:1eb49362a607 435 case 16:Ph2();break;
simontruelove 14:1eb49362a607 436 case 15:Ph1();break;
simontruelove 14:1eb49362a607 437 case 14:Ph1();break;
simontruelove 14:1eb49362a607 438 case 13:Ph1();break;
simontruelove 14:1eb49362a607 439 case 12:Ph1();break;
simontruelove 14:1eb49362a607 440 case 11:Ph1();break;
simontruelove 14:1eb49362a607 441 case 10:Ph1();break;
simontruelove 14:1eb49362a607 442 case 9:Ph1();break;
simontruelove 14:1eb49362a607 443 case 8:Ph1();break;
simontruelove 14:1eb49362a607 444 case 7:Ph1();break;
simontruelove 14:1eb49362a607 445 case 6:Ph1();break;
simontruelove 14:1eb49362a607 446 case 5:Ph1();break;
simontruelove 14:1eb49362a607 447 case 4:Ph1();break;
simontruelove 13:da9d3fbbe407 448 case 3:Ph1();break;
simontruelove 13:da9d3fbbe407 449 case 2:Ph1();break;
simontruelove 6:f7028034aabb 450 case 1:Ph1();break;
simontruelove 6:f7028034aabb 451 case 0:Ph1();break;
simontruelove 5:4e5c644d5cc3 452 default:break;
simontruelove 5:4e5c644d5cc3 453 }
simontruelove 14:1eb49362a607 454 /*if(wheel.getYay()==1)
simontruelove 14:1eb49362a607 455 {
simontruelove 14:1eb49362a607 456 pc.printf("3 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 457 }*/
simontruelove 12:cbea987a3ec4 458 if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
simontruelove 5:4e5c644d5cc3 459 {
simontruelove 5:4e5c644d5cc3 460 RPM();
simontruelove 9:061600a6c750 461 VelocityLoop();
simontruelove 14:1eb49362a607 462 slowloop++;
simontruelove 14:1eb49362a607 463 if(slowloop>rpm)
simontruelove 14:1eb49362a607 464 {
simontruelove 14:1eb49362a607 465 ReadKType();
simontruelove 14:1eb49362a607 466 slowloop=0;
simontruelove 14:1eb49362a607 467 }
simontruelove 5:4e5c644d5cc3 468 }
simontruelove 3:4249dbdf7ed3 469 }
simontruelove 11:74eeb8871fe6 470 while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
simontruelove 5:4e5c644d5cc3 471 {
simontruelove 5:4e5c644d5cc3 472 GetChar();
simontruelove 14:1eb49362a607 473 StateB = (enc+wheel.getPulses()+StateA+AdjACW)%s;
simontruelove 6:f7028034aabb 474 //pc.printf("StateA= %i\r", StateA);
simontruelove 14:1eb49362a607 475 //pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 476 switch(StateB)
simontruelove 5:4e5c644d5cc3 477 {
simontruelove 14:1eb49362a607 478 case 63:Ph4();break;
simontruelove 14:1eb49362a607 479 case 62:Ph4();break;
simontruelove 14:1eb49362a607 480 case 61:Ph4();break;
simontruelove 14:1eb49362a607 481 case 60:Ph4();break;
simontruelove 14:1eb49362a607 482 case 59:Ph4();break;
simontruelove 14:1eb49362a607 483 case 58:Ph4();break;
simontruelove 14:1eb49362a607 484 case 57:Ph4();break;
simontruelove 14:1eb49362a607 485 case 56:Ph4();break;
simontruelove 14:1eb49362a607 486 case 55:Ph4();break;
simontruelove 14:1eb49362a607 487 case 54:Ph4();break;
simontruelove 14:1eb49362a607 488 case 53:Ph4();break;
simontruelove 14:1eb49362a607 489 case 52:Ph4();break;
simontruelove 14:1eb49362a607 490 case 51:Ph4();break;
simontruelove 14:1eb49362a607 491 case 50:Ph4();break;
simontruelove 14:1eb49362a607 492 case 49:Ph4();break;
simontruelove 14:1eb49362a607 493 case 48:Ph4();break;
simontruelove 14:1eb49362a607 494 case 47:Ph3();break;
simontruelove 14:1eb49362a607 495 case 46:Ph3();break;
simontruelove 14:1eb49362a607 496 case 45:Ph3();break;
simontruelove 14:1eb49362a607 497 case 44:Ph3();break;
simontruelove 14:1eb49362a607 498 case 43:Ph3();break;
simontruelove 14:1eb49362a607 499 case 42:Ph3();break;
simontruelove 14:1eb49362a607 500 case 41:Ph3();break;
simontruelove 14:1eb49362a607 501 case 40:Ph3();break;
simontruelove 14:1eb49362a607 502 case 39:Ph3();break;
simontruelove 14:1eb49362a607 503 case 38:Ph3();break;
simontruelove 14:1eb49362a607 504 case 37:Ph3();break;
simontruelove 14:1eb49362a607 505 case 36:Ph3();break;
simontruelove 14:1eb49362a607 506 case 35:Ph3();break;
simontruelove 14:1eb49362a607 507 case 34:Ph3();break;
simontruelove 14:1eb49362a607 508 case 33:Ph3();break;
simontruelove 14:1eb49362a607 509 case 32:Ph3();break;
simontruelove 14:1eb49362a607 510 case 31:Ph2();break;
simontruelove 14:1eb49362a607 511 case 30:Ph2();break;
simontruelove 14:1eb49362a607 512 case 29:Ph2();break;
simontruelove 14:1eb49362a607 513 case 28:Ph2();break;
simontruelove 14:1eb49362a607 514 case 27:Ph2();break;
simontruelove 14:1eb49362a607 515 case 26:Ph2();break;
simontruelove 14:1eb49362a607 516 case 25:Ph2();break;
simontruelove 14:1eb49362a607 517 case 24:Ph2();break;
simontruelove 14:1eb49362a607 518 case 23:Ph2();break;
simontruelove 14:1eb49362a607 519 case 22:Ph2();break;
simontruelove 14:1eb49362a607 520 case 21:Ph2();break;
simontruelove 14:1eb49362a607 521 case 20:Ph2();break;
simontruelove 14:1eb49362a607 522 case 19:Ph2();break;
simontruelove 14:1eb49362a607 523 case 18:Ph2();break;
simontruelove 14:1eb49362a607 524 case 17:Ph2();break;
simontruelove 14:1eb49362a607 525 case 16:Ph2();break;
simontruelove 14:1eb49362a607 526 case 15:Ph1();break;
simontruelove 14:1eb49362a607 527 case 14:Ph1();break;
simontruelove 14:1eb49362a607 528 case 13:Ph1();break;
simontruelove 14:1eb49362a607 529 case 12:Ph1();break;
simontruelove 14:1eb49362a607 530 case 11:Ph1();break;
simontruelove 14:1eb49362a607 531 case 10:Ph1();break;
simontruelove 14:1eb49362a607 532 case 9:Ph1();break;
simontruelove 14:1eb49362a607 533 case 8:Ph1();break;
simontruelove 14:1eb49362a607 534 case 7:Ph1();break;
simontruelove 14:1eb49362a607 535 case 6:Ph1();break;
simontruelove 14:1eb49362a607 536 case 5:Ph1();break;
simontruelove 14:1eb49362a607 537 case 4:Ph1();break;
simontruelove 13:da9d3fbbe407 538 case 3:Ph1();break;
simontruelove 13:da9d3fbbe407 539 case 2:Ph1();break;
simontruelove 6:f7028034aabb 540 case 1:Ph1();break;
simontruelove 6:f7028034aabb 541 case 0:Ph1();break;
simontruelove 14:1eb49362a607 542 default:break;
simontruelove 5:4e5c644d5cc3 543 }
simontruelove 14:1eb49362a607 544 /*if(wheel.getYay()==1)
simontruelove 14:1eb49362a607 545 {
simontruelove 14:1eb49362a607 546 pc.printf("4 StateB= %i, Pulses= %i, Revs= %i\n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 547 }*/
simontruelove 14:1eb49362a607 548 if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
simontruelove 5:4e5c644d5cc3 549 {
simontruelove 5:4e5c644d5cc3 550 RPM();
simontruelove 9:061600a6c750 551 VelocityLoop();
simontruelove 14:1eb49362a607 552 slowloop++;
simontruelove 14:1eb49362a607 553 if(slowloop>rpm)
simontruelove 14:1eb49362a607 554 {
simontruelove 14:1eb49362a607 555 ReadKType();
simontruelove 14:1eb49362a607 556 slowloop=0;
simontruelove 14:1eb49362a607 557 }
simontruelove 11:74eeb8871fe6 558 }
simontruelove 5:4e5c644d5cc3 559 }
simontruelove 11:74eeb8871fe6 560 while(temp>(T-1))
simontruelove 11:74eeb8871fe6 561 {
simontruelove 11:74eeb8871fe6 562 Initialisation();
simontruelove 11:74eeb8871fe6 563 pc.printf("Motor Over Temp\n\r");
simontruelove 11:74eeb8871fe6 564 while(1)
simontruelove 11:74eeb8871fe6 565 {
simontruelove 11:74eeb8871fe6 566 ReadKType();
simontruelove 11:74eeb8871fe6 567 pc.printf("%f\r",temp);
simontruelove 11:74eeb8871fe6 568 wait(1);
simontruelove 11:74eeb8871fe6 569 if(temp<T-20)
simontruelove 11:74eeb8871fe6 570 {
simontruelove 11:74eeb8871fe6 571 pc.printf("Motor Back Online\n\r");
simontruelove 11:74eeb8871fe6 572 break;
simontruelove 11:74eeb8871fe6 573 }
simontruelove 11:74eeb8871fe6 574 }
simontruelove 11:74eeb8871fe6 575 }
simontruelove 1:0191658b6ff4 576 }
simontruelove 1:0191658b6ff4 577 }
simontruelove 1:0191658b6ff4 578 void StepCW(void) //Square wave switching
simontruelove 1:0191658b6ff4 579 {
simontruelove 14:1eb49362a607 580 /*Ph1();
simontruelove 3:4249dbdf7ed3 581 wait(x);
simontruelove 14:1eb49362a607 582 //pc.printf("1 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 583 Ph12();
simontruelove 14:1eb49362a607 584 wait(y);
simontruelove 14:1eb49362a607 585 //pc.printf("12 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 1:0191658b6ff4 586 Ph2();
simontruelove 3:4249dbdf7ed3 587 wait(x);
simontruelove 14:1eb49362a607 588 //pc.printf("2 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 589 Ph23();
simontruelove 14:1eb49362a607 590 wait(y);
simontruelove 14:1eb49362a607 591 //pc.printf("23 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 1:0191658b6ff4 592 Ph3();
simontruelove 3:4249dbdf7ed3 593 wait(x);
simontruelove 14:1eb49362a607 594 //pc.printf("3 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 595 Ph34();
simontruelove 14:1eb49362a607 596 wait(y);
simontruelove 14:1eb49362a607 597 //pc.printf("34 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 598 Ph4();
simontruelove 14:1eb49362a607 599 wait(x);
simontruelove 14:1eb49362a607 600 //pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 601 Ph41();
simontruelove 14:1eb49362a607 602 wait(y);
simontruelove 14:1eb49362a607 603 //pc.printf("41 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 604 */
simontruelove 1:0191658b6ff4 605 Ph4();
simontruelove 3:4249dbdf7ed3 606 wait(x);
simontruelove 14:1eb49362a607 607 //pc.printf("4 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 608 Ph34();
simontruelove 14:1eb49362a607 609 wait(y);
simontruelove 14:1eb49362a607 610 //pc.printf("34 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 611 Ph3();
simontruelove 14:1eb49362a607 612 wait(x);
simontruelove 14:1eb49362a607 613 //pc.printf("3 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 614 Ph23();
simontruelove 14:1eb49362a607 615 wait(y);
simontruelove 14:1eb49362a607 616 //pc.printf("23 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 617 Ph2();
simontruelove 14:1eb49362a607 618 wait(x);
simontruelove 14:1eb49362a607 619 //pc.printf("2 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 620 Ph12();
simontruelove 14:1eb49362a607 621 wait(y);
simontruelove 14:1eb49362a607 622 //pc.printf("12 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 623 Ph1();
simontruelove 14:1eb49362a607 624 wait(x);
simontruelove 14:1eb49362a607 625 //pc.printf("1 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 626 Ph41();
simontruelove 14:1eb49362a607 627 wait(y);
simontruelove 14:1eb49362a607 628 //pc.printf("41 StateA= %i, Pulses= %i, Revs= %i\n\r", StateA,wheel.getPulses(),wheel.getRevolutions());
simontruelove 13:da9d3fbbe407 629 }
simontruelove 13:da9d3fbbe407 630
simontruelove 13:da9d3fbbe407 631 void Ph0(void)
simontruelove 13:da9d3fbbe407 632 {
simontruelove 13:da9d3fbbe407 633 Phase1.write(0);
simontruelove 13:da9d3fbbe407 634 Phase2.write(0);
simontruelove 13:da9d3fbbe407 635 Phase3.write(0);
simontruelove 13:da9d3fbbe407 636 Phase4.write(0);
simontruelove 13:da9d3fbbe407 637 //wait(x);
simontruelove 13:da9d3fbbe407 638 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 1:0191658b6ff4 639 }
simontruelove 1:0191658b6ff4 640
simontruelove 1:0191658b6ff4 641 void Ph1(void)
simontruelove 1:0191658b6ff4 642 {
simontruelove 9:061600a6c750 643 Phase1.write(duty);
simontruelove 9:061600a6c750 644 Phase2.write(0);
simontruelove 9:061600a6c750 645 Phase3.write(0);
simontruelove 9:061600a6c750 646 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 647 //wait(x);
simontruelove 3:4249dbdf7ed3 648 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 649 }
simontruelove 3:4249dbdf7ed3 650
simontruelove 3:4249dbdf7ed3 651 void Ph12 (void)
simontruelove 3:4249dbdf7ed3 652 {
simontruelove 9:061600a6c750 653 Phase1.write(duty);
simontruelove 9:061600a6c750 654 Phase2.write(duty);
simontruelove 9:061600a6c750 655 Phase3.write(0);
simontruelove 9:061600a6c750 656 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 657 //wait(y);
simontruelove 1:0191658b6ff4 658 }
simontruelove 1:0191658b6ff4 659
simontruelove 1:0191658b6ff4 660 void Ph2(void)
simontruelove 1:0191658b6ff4 661 {
simontruelove 9:061600a6c750 662 Phase1.write(0);
simontruelove 9:061600a6c750 663 Phase2.write(duty);
simontruelove 9:061600a6c750 664 Phase3.write(0);
simontruelove 9:061600a6c750 665 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 666 //wait(x);
simontruelove 3:4249dbdf7ed3 667 //pc.printf("Phase 2 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 668 }
simontruelove 3:4249dbdf7ed3 669
simontruelove 3:4249dbdf7ed3 670 void Ph23 (void)
simontruelove 3:4249dbdf7ed3 671 {
simontruelove 9:061600a6c750 672 Phase1.write(0);
simontruelove 9:061600a6c750 673 Phase2.write(duty);
simontruelove 9:061600a6c750 674 Phase3.write(duty);
simontruelove 9:061600a6c750 675 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 676 //wait(y);
simontruelove 1:0191658b6ff4 677 }
simontruelove 0:634dd505dace 678
simontruelove 1:0191658b6ff4 679 void Ph3(void)
simontruelove 1:0191658b6ff4 680 {
simontruelove 9:061600a6c750 681 Phase1.write(0);
simontruelove 9:061600a6c750 682 Phase2.write(0);
simontruelove 9:061600a6c750 683 Phase3.write(duty);
simontruelove 9:061600a6c750 684 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 685 //wait(x);
simontruelove 3:4249dbdf7ed3 686 //pc.printf("Phase 3 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 687 }
simontruelove 3:4249dbdf7ed3 688 void Ph34 (void)
simontruelove 3:4249dbdf7ed3 689 {
simontruelove 9:061600a6c750 690 Phase1.write(0);
simontruelove 9:061600a6c750 691 Phase2.write(0);
simontruelove 9:061600a6c750 692 Phase3.write(duty);
simontruelove 9:061600a6c750 693 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 694 //wait(y);
simontruelove 1:0191658b6ff4 695 }
simontruelove 1:0191658b6ff4 696
simontruelove 1:0191658b6ff4 697 void Ph4(void)
simontruelove 1:0191658b6ff4 698 {
simontruelove 9:061600a6c750 699 Phase1.write(0);
simontruelove 9:061600a6c750 700 Phase2.write(0);
simontruelove 9:061600a6c750 701 Phase3.write(0);
simontruelove 9:061600a6c750 702 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 703 //wait(x);
simontruelove 3:4249dbdf7ed3 704 //pc.printf("Phase 4 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 705 }
simontruelove 3:4249dbdf7ed3 706
simontruelove 3:4249dbdf7ed3 707 void Ph41 (void)
simontruelove 3:4249dbdf7ed3 708 {
simontruelove 9:061600a6c750 709 Phase1.write(duty);
simontruelove 9:061600a6c750 710 Phase2.write(0);
simontruelove 9:061600a6c750 711 Phase3.write(0);
simontruelove 9:061600a6c750 712 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 713 //wait(y);
simontruelove 3:4249dbdf7ed3 714 }
simontruelove 3:4249dbdf7ed3 715
simontruelove 10:808cb9052f14 716 void Initialisation (void) //Turn everything off
simontruelove 3:4249dbdf7ed3 717 {
simontruelove 9:061600a6c750 718 Phase1.write(0);
simontruelove 9:061600a6c750 719 Phase2.write(0);
simontruelove 9:061600a6c750 720 Phase3.write(0);
simontruelove 9:061600a6c750 721 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 722 led1 = 0;
simontruelove 3:4249dbdf7ed3 723 led2 = 0;
simontruelove 3:4249dbdf7ed3 724 led3 = 0;
simontruelove 3:4249dbdf7ed3 725 led4 = 0;
simontruelove 12:cbea987a3ec4 726 UnUsedPhase1=0;
simontruelove 12:cbea987a3ec4 727 UnUsedPhase2=0;
simontruelove 3:4249dbdf7ed3 728 wheel.ResetYay();
simontruelove 14:1eb49362a607 729 wheel.QEI::reset();
simontruelove 4:3aedc9246ae4 730 }
simontruelove 4:3aedc9246ae4 731
simontruelove 10:808cb9052f14 732 void GetChar (void) //read keyboard strikes with terraterm
simontruelove 4:3aedc9246ae4 733 { if (pc.readable())
simontruelove 4:3aedc9246ae4 734 {
simontruelove 4:3aedc9246ae4 735 c = pc.getc();
simontruelove 10:808cb9052f14 736 if(c == 'z') //turn on led1 causes CW operation
simontruelove 4:3aedc9246ae4 737 {
simontruelove 14:1eb49362a607 738 //AdjCW = 0;
simontruelove 14:1eb49362a607 739 //AdjACW = 10;
simontruelove 4:3aedc9246ae4 740 led1 = !led1;
simontruelove 4:3aedc9246ae4 741 led2 = 0;
simontruelove 13:da9d3fbbe407 742 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 743 }
simontruelove 10:808cb9052f14 744 if(c == 'x') //turn on led2 causes ACW operation
simontruelove 4:3aedc9246ae4 745 {
simontruelove 14:1eb49362a607 746 //AdjCW = 0;
simontruelove 14:1eb49362a607 747 //AdjACW = 10;
simontruelove 4:3aedc9246ae4 748 led1 = 0;
simontruelove 4:3aedc9246ae4 749 led2 = !led2 ;
simontruelove 10:808cb9052f14 750 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 751 }
simontruelove 10:808cb9052f14 752 if(c == 'q') //Increases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 753 {
simontruelove 9:061600a6c750 754 //duty = duty + 0.0001;
simontruelove 13:da9d3fbbe407 755 SetPoint=SetPoint+5;
simontruelove 14:1eb49362a607 756 if (SetPoint >3000)
simontruelove 10:808cb9052f14 757 {
simontruelove 14:1eb49362a607 758 SetPoint = 3000;
simontruelove 10:808cb9052f14 759 }
simontruelove 10:808cb9052f14 760 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 7:b8de1529c7fc 761 }
simontruelove 10:808cb9052f14 762 if(c == 'a') //Decreases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 763 {
simontruelove 9:061600a6c750 764 //duty = duty - 0.0001;
simontruelove 13:da9d3fbbe407 765 SetPoint=SetPoint-5;
simontruelove 10:808cb9052f14 766 if (SetPoint <50)
simontruelove 9:061600a6c750 767 {
simontruelove 10:808cb9052f14 768 SetPoint = 50;
simontruelove 9:061600a6c750 769 }
simontruelove 10:808cb9052f14 770 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 771 }
simontruelove 14:1eb49362a607 772 if(c == 'w') //Increases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 773 {
simontruelove 14:1eb49362a607 774 duty = duty + 0.1;
simontruelove 14:1eb49362a607 775 /*SetPoint=SetPoint+5;
simontruelove 14:1eb49362a607 776 if (SetPoint >2200)
simontruelove 14:1eb49362a607 777 {
simontruelove 14:1eb49362a607 778 SetPoint = 2200;
simontruelove 14:1eb49362a607 779 }*/
simontruelove 14:1eb49362a607 780 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 781 }
simontruelove 14:1eb49362a607 782 if(c == 's') //Decreases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 783 {
simontruelove 14:1eb49362a607 784 duty = duty - 0.1;
simontruelove 14:1eb49362a607 785 /*SetPoint=SetPoint-5;
simontruelove 14:1eb49362a607 786 if (SetPoint <50)
simontruelove 14:1eb49362a607 787 {
simontruelove 14:1eb49362a607 788 SetPoint = 50;
simontruelove 14:1eb49362a607 789 }*/
simontruelove 14:1eb49362a607 790 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 791 }
simontruelove 14:1eb49362a607 792 if(c == 'e') //Increases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 793 {
simontruelove 14:1eb49362a607 794 AdjDiff = AdjDiff * 10;
simontruelove 14:1eb49362a607 795 /*SetPoint=SetPoint+5;
simontruelove 14:1eb49362a607 796 if (SetPoint >2200)
simontruelove 14:1eb49362a607 797 {
simontruelove 14:1eb49362a607 798 SetPoint = 2200;
simontruelove 14:1eb49362a607 799 }*/
simontruelove 14:1eb49362a607 800 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 801 }
simontruelove 14:1eb49362a607 802 if(c == 'd') //Decreases setpoint used in Velocity loop
simontruelove 14:1eb49362a607 803 {
simontruelove 14:1eb49362a607 804 AdjDiff = AdjDiff / 10;
simontruelove 14:1eb49362a607 805 /*SetPoint=SetPoint-5;
simontruelove 14:1eb49362a607 806 if (SetPoint <50)
simontruelove 14:1eb49362a607 807 {
simontruelove 14:1eb49362a607 808 SetPoint = 50;
simontruelove 14:1eb49362a607 809 }*/
simontruelove 14:1eb49362a607 810 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 14:1eb49362a607 811 }
simontruelove 10:808cb9052f14 812 if(c== 'o')
simontruelove 10:808cb9052f14 813 {
simontruelove 10:808cb9052f14 814 AdjCW = AdjCW+1;
simontruelove 14:1eb49362a607 815 if (AdjCW >(s-1))
simontruelove 10:808cb9052f14 816 {
simontruelove 10:808cb9052f14 817 AdjCW = 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== 'k')
simontruelove 10:808cb9052f14 822 {
simontruelove 10:808cb9052f14 823 AdjCW = AdjCW-1;
simontruelove 10:808cb9052f14 824 if (AdjCW <0)
simontruelove 10:808cb9052f14 825 {
simontruelove 14:1eb49362a607 826 AdjCW = (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 10:808cb9052f14 830 if(c== 'p')
simontruelove 10:808cb9052f14 831 {
simontruelove 10:808cb9052f14 832 AdjACW = AdjACW+1;
simontruelove 14:1eb49362a607 833 if (AdjACW >(s-1))
simontruelove 10:808cb9052f14 834 {
simontruelove 10:808cb9052f14 835 AdjACW = 0;
simontruelove 10:808cb9052f14 836 }
simontruelove 10:808cb9052f14 837 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 838 }
simontruelove 10:808cb9052f14 839 if(c== 'l')
simontruelove 10:808cb9052f14 840 {
simontruelove 10:808cb9052f14 841 AdjACW = AdjACW-1;
simontruelove 10:808cb9052f14 842 if (AdjACW <0)
simontruelove 10:808cb9052f14 843 {
simontruelove 14:1eb49362a607 844 AdjACW = (s-1);
simontruelove 10:808cb9052f14 845 }
simontruelove 10:808cb9052f14 846 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 847 }
simontruelove 10:808cb9052f14 848 if(c=='0')
simontruelove 10:808cb9052f14 849 {
simontruelove 14:1eb49362a607 850 pc.printf("%i, %.5f, %i, %i, %i, %f \n\r", SetPoint, duty, AdjCW, AdjACW, rpm, AdjDiff);
simontruelove 8:2e690f407ec6 851 }
simontruelove 11:74eeb8871fe6 852 if(c=='t')
simontruelove 11:74eeb8871fe6 853 {
simontruelove 12:cbea987a3ec4 854 pc.printf("%.0f C\n\r",temp);
simontruelove 11:74eeb8871fe6 855 }
simontruelove 14:1eb49362a607 856 if(c=='y')
simontruelove 14:1eb49362a607 857 {
simontruelove 14:1eb49362a607 858 pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \n\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 14:1eb49362a607 859 }
simontruelove 4:3aedc9246ae4 860 }
simontruelove 5:4e5c644d5cc3 861 }
simontruelove 5:4e5c644d5cc3 862
simontruelove 5:4e5c644d5cc3 863 void RPM (void)
simontruelove 5:4e5c644d5cc3 864 {
simontruelove 14:1eb49362a607 865 wheel.ResetWhoop(); //PulseCount2_==400 x 4, x4 encoding, whoop_=1;
simontruelove 14:1eb49362a607 866 TimePerClick = (t.read_us()); //read timer in microseconds
simontruelove 14:1eb49362a607 867 t.reset(); //reset timer
simontruelove 14:1eb49362a607 868 TimePerRev = enc * TimePerClick / z; //z = 400 (PulseCount2_==1600)
simontruelove 14:1eb49362a607 869 TimePerRev = TimePerRev / 1000000; // 1microsecond = 0.000001s
simontruelove 14:1eb49362a607 870 RPS = 1 / TimePerRev; //inverse to convert SPR to RPS
simontruelove 14:1eb49362a607 871 rpm = RPS * 60; //x 60 to convert RPS to RPM
simontruelove 14:1eb49362a607 872 //Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
simontruelove 14:1eb49362a607 873 //ReadKType();
simontruelove 14:1eb49362a607 874 if(rpm < 300)
simontruelove 14:1eb49362a607 875 {
simontruelove 14:1eb49362a607 876 AdjCW = 57;
simontruelove 14:1eb49362a607 877 AdjACW = 12;
simontruelove 14:1eb49362a607 878 }
simontruelove 14:1eb49362a607 879 if(rpm > 299 && rpm <400)
simontruelove 14:1eb49362a607 880 {
simontruelove 14:1eb49362a607 881 AdjCW = 62;
simontruelove 14:1eb49362a607 882 AdjACW = 11;
simontruelove 14:1eb49362a607 883 }
simontruelove 14:1eb49362a607 884 if(rpm > 399 and rpm < 525)
simontruelove 14:1eb49362a607 885 {
simontruelove 14:1eb49362a607 886 AdjCW = 63;
simontruelove 14:1eb49362a607 887 AdjACW = 10;
simontruelove 14:1eb49362a607 888 }
simontruelove 14:1eb49362a607 889 if(rpm > 524 and rpm < 625)
simontruelove 13:da9d3fbbe407 890 {
simontruelove 13:da9d3fbbe407 891 AdjCW = 0;
simontruelove 14:1eb49362a607 892 AdjACW = 9;
simontruelove 14:1eb49362a607 893 }
simontruelove 14:1eb49362a607 894 if(rpm > 624 and rpm < 725)
simontruelove 14:1eb49362a607 895 {
simontruelove 14:1eb49362a607 896 AdjCW = 1;
simontruelove 14:1eb49362a607 897 AdjACW = 8;
simontruelove 13:da9d3fbbe407 898 }
simontruelove 14:1eb49362a607 899 if(rpm > 724 and rpm < 850)
simontruelove 14:1eb49362a607 900 {
simontruelove 14:1eb49362a607 901 AdjCW = 2;
simontruelove 14:1eb49362a607 902 AdjACW = 7;
simontruelove 14:1eb49362a607 903 }
simontruelove 14:1eb49362a607 904 if(rpm > 849 and rpm < 975)
simontruelove 13:da9d3fbbe407 905 {
simontruelove 13:da9d3fbbe407 906 AdjCW = 3;
simontruelove 14:1eb49362a607 907 AdjACW = 6;
simontruelove 13:da9d3fbbe407 908 }
simontruelove 14:1eb49362a607 909 if(rpm > 974 and rpm < 1150)
simontruelove 13:da9d3fbbe407 910 {
simontruelove 13:da9d3fbbe407 911 AdjCW = 4;
simontruelove 14:1eb49362a607 912 AdjACW = 5;
simontruelove 14:1eb49362a607 913 }
simontruelove 14:1eb49362a607 914 if(rpm > 1149 and rpm < 1375)
simontruelove 13:da9d3fbbe407 915 {
simontruelove 13:da9d3fbbe407 916 AdjCW = 5;
simontruelove 14:1eb49362a607 917 AdjACW = 4;
simontruelove 14:1eb49362a607 918 }
simontruelove 14:1eb49362a607 919 if(rpm > 1374 and rpm < 1725)
simontruelove 14:1eb49362a607 920 {
simontruelove 14:1eb49362a607 921 AdjCW = 6;
simontruelove 14:1eb49362a607 922 AdjACW = 3;
simontruelove 14:1eb49362a607 923 }
simontruelove 14:1eb49362a607 924 if(rpm > 1724 and rpm < 2150)
simontruelove 14:1eb49362a607 925 {
simontruelove 14:1eb49362a607 926 AdjCW = 7;
simontruelove 14:1eb49362a607 927 AdjACW = 2;
simontruelove 13:da9d3fbbe407 928 }
simontruelove 14:1eb49362a607 929 if(rpm > 2149 and rpm < 2525)
simontruelove 14:1eb49362a607 930 {
simontruelove 14:1eb49362a607 931 AdjCW = 8;
simontruelove 14:1eb49362a607 932 AdjACW = 1;
simontruelove 14:1eb49362a607 933 }
simontruelove 14:1eb49362a607 934 if(rpm > 2524 and rpm < 2600)
simontruelove 14:1eb49362a607 935 {
simontruelove 14:1eb49362a607 936 AdjCW = 9;
simontruelove 14:1eb49362a607 937 AdjACW = 0;
simontruelove 14:1eb49362a607 938 }
simontruelove 14:1eb49362a607 939 if(rpm > 2599 and rpm < 2650)
simontruelove 13:da9d3fbbe407 940 {
simontruelove 14:1eb49362a607 941 AdjCW = 10;
simontruelove 14:1eb49362a607 942 AdjACW = 63;
simontruelove 14:1eb49362a607 943 }
simontruelove 14:1eb49362a607 944 if(rpm > 2649 and rpm < 2700)
simontruelove 14:1eb49362a607 945 {
simontruelove 14:1eb49362a607 946 AdjCW = 11;
simontruelove 14:1eb49362a607 947 AdjACW = 62;
simontruelove 14:1eb49362a607 948 }
simontruelove 14:1eb49362a607 949 if(rpm > 2699 and rpm < 2750)
simontruelove 14:1eb49362a607 950 {
simontruelove 14:1eb49362a607 951 AdjCW = 12;
simontruelove 14:1eb49362a607 952 AdjACW = 61;
simontruelove 13:da9d3fbbe407 953 }
simontruelove 14:1eb49362a607 954 if(rpm > 2749 and rpm < 2825)
simontruelove 14:1eb49362a607 955 {
simontruelove 14:1eb49362a607 956 AdjCW = 13;
simontruelove 14:1eb49362a607 957 AdjACW = 60;
simontruelove 14:1eb49362a607 958 }
simontruelove 14:1eb49362a607 959 if(rpm > 2824 and rpm < 2875)
simontruelove 14:1eb49362a607 960 {
simontruelove 14:1eb49362a607 961 AdjCW = 14;
simontruelove 14:1eb49362a607 962 AdjACW = 59;
simontruelove 14:1eb49362a607 963 }
simontruelove 14:1eb49362a607 964 if(rpm > 2874)
simontruelove 14:1eb49362a607 965 {
simontruelove 14:1eb49362a607 966 AdjCW = 15;
simontruelove 14:1eb49362a607 967 AdjACW = 58;
simontruelove 14:1eb49362a607 968 }
simontruelove 14:1eb49362a607 969 //pc.printf("RPS = %i \r", RPS);
simontruelove 6:f7028034aabb 970 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 7:b8de1529c7fc 971 }
simontruelove 7:b8de1529c7fc 972
simontruelove 7:b8de1529c7fc 973 void VelocityLoop (void)
simontruelove 7:b8de1529c7fc 974 {
simontruelove 14:1eb49362a607 975 diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
simontruelove 14:1eb49362a607 976 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 14:1eb49362a607 977 /*if(duty > 0.8)
simontruelove 14:1eb49362a607 978 {
simontruelove 14:1eb49362a607 979 duty = duty + (diff*0.000001); //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 14:1eb49362a607 980 }
simontruelove 14:1eb49362a607 981 if(0.5 < duty < 0.8)
simontruelove 14:1eb49362a607 982 {
simontruelove 14:1eb49362a607 983 duty = duty + (diff*0.005); //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 14:1eb49362a607 984 }
simontruelove 14:1eb49362a607 985 if(duty < 0.5)
simontruelove 14:1eb49362a607 986 {
simontruelove 14:1eb49362a607 987 duty = duty + (diff*0.001); //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 14:1eb49362a607 988 }
simontruelove 14:1eb49362a607 989 /*if(0 < rpm < 500)
simontruelove 14:1eb49362a607 990 {
simontruelove 14:1eb49362a607 991 duty = duty + (diff*0.0001); //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 14:1eb49362a607 992 }
simontruelove 14:1eb49362a607 993 if(500 < rpm < 1000)
simontruelove 14:1eb49362a607 994 {
simontruelove 14:1eb49362a607 995 duty = duty + (diff*0.00008); //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 14:1eb49362a607 996 }
simontruelove 14:1eb49362a607 997 if(1000 < rpm < 1500)
simontruelove 14:1eb49362a607 998 {
simontruelove 14:1eb49362a607 999 duty = duty + (diff*0.00006); //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 14:1eb49362a607 1000 }
simontruelove 14:1eb49362a607 1001 if(1500 < rpm < 2000)
simontruelove 14:1eb49362a607 1002 {
simontruelove 14:1eb49362a607 1003 duty = duty + (diff*0.00004); //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 14:1eb49362a607 1004 }
simontruelove 14:1eb49362a607 1005 if(2000 < rpm < 2500)
simontruelove 14:1eb49362a607 1006 {
simontruelove 14:1eb49362a607 1007 duty = duty + (diff*0.00002); //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 14:1eb49362a607 1008 }
simontruelove 14:1eb49362a607 1009 if(2500 < rpm < 3000)
simontruelove 14:1eb49362a607 1010 {
simontruelove 14:1eb49362a607 1011 duty = duty + (diff*0.00001); //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 14:1eb49362a607 1012 }*/
simontruelove 14:1eb49362a607 1013 if (duty > 1) //limits for duty. Motor will not operate below 0.96. 1 = max
simontruelove 14:1eb49362a607 1014 {
simontruelove 9:061600a6c750 1015 duty = 1;
simontruelove 7:b8de1529c7fc 1016 }
simontruelove 14:1eb49362a607 1017 if (duty <0.1) //3A min duty 0.96, 6.5A min duty 0.4
simontruelove 10:808cb9052f14 1018 {
simontruelove 14:1eb49362a607 1019 duty = 0.1;
simontruelove 7:b8de1529c7fc 1020 }
simontruelove 10:808cb9052f14 1021 //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 1022 }
simontruelove 11:74eeb8871fe6 1023
simontruelove 11:74eeb8871fe6 1024 void ReadKType(void)
simontruelove 11:74eeb8871fe6 1025 {
simontruelove 11:74eeb8871fe6 1026 int i = 0;
simontruelove 11:74eeb8871fe6 1027 int Readout = 0;
simontruelove 11:74eeb8871fe6 1028
simontruelove 11:74eeb8871fe6 1029 cs1 = 0;
simontruelove 14:1eb49362a607 1030 SerialClock = 0; //set clock to 0
simontruelove 14:1eb49362a607 1031 wait_ms(1);
simontruelove 14:1eb49362a607 1032 SerialClock = 1; //clock once to set to the 13 bit temp data
simontruelove 12:cbea987a3ec4 1033 wait_ms(0.1);
simontruelove 11:74eeb8871fe6 1034 SerialClock = 0;
simontruelove 12:cbea987a3ec4 1035 wait_ms(0.1);
simontruelove 11:74eeb8871fe6 1036
simontruelove 11:74eeb8871fe6 1037 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 1038 {
simontruelove 11:74eeb8871fe6 1039 if(DOut == 1) //check data, store results in readout
simontruelove 11:74eeb8871fe6 1040 {
simontruelove 12:cbea987a3ec4 1041 Readout |= (1<<i);
simontruelove 11:74eeb8871fe6 1042 }
simontruelove 11:74eeb8871fe6 1043 else
simontruelove 11:74eeb8871fe6 1044 {
simontruelove 11:74eeb8871fe6 1045 Readout |= (0<<i);
simontruelove 11:74eeb8871fe6 1046 }
simontruelove 12:cbea987a3ec4 1047
simontruelove 11:74eeb8871fe6 1048 SerialClock = 1; //clock to the next bit
simontruelove 12:cbea987a3ec4 1049 wait_ms(0.1);
simontruelove 11:74eeb8871fe6 1050 SerialClock = 0;
simontruelove 12:cbea987a3ec4 1051 wait_ms(0.1);
simontruelove 11:74eeb8871fe6 1052 }
simontruelove 11:74eeb8871fe6 1053 temp = Readout * 0.125; //get the real temp value which is a float
simontruelove 11:74eeb8871fe6 1054 //pc.printf("%f\n\r",temp);
simontruelove 11:74eeb8871fe6 1055 Readout = 0;
simontruelove 11:74eeb8871fe6 1056 cs1 = 1;
simontruelove 3:4249dbdf7ed3 1057 }