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@10:808cb9052f14, 2018-12-14 (annotated)

Committer:
simontruelove
Date:
Fri Dec 14 11:00:47 2018 +0000
Revision:
10:808cb9052f14
Parent:
9:061600a6c750
Child:
11:74eeb8871fe6
Auto phase lead added

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 1:0191658b6ff4 6 void Ph1(void);
simontruelove 3:4249dbdf7ed3 7 void Ph12 (void);
simontruelove 1:0191658b6ff4 8 void Ph2(void);
simontruelove 3:4249dbdf7ed3 9 void Ph23 (void);
simontruelove 1:0191658b6ff4 10 void Ph3(void);
simontruelove 3:4249dbdf7ed3 11 void Ph34 (void);
simontruelove 1:0191658b6ff4 12 void Ph4(void);
simontruelove 3:4249dbdf7ed3 13 void Ph41 (void);
simontruelove 4:3aedc9246ae4 14 void GetChar (void);
simontruelove 5:4e5c644d5cc3 15 void RPM (void);
simontruelove 7:b8de1529c7fc 16 void VelocityLoop (void);
simontruelove 1:0191658b6ff4 17
simontruelove 10:808cb9052f14 18 Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
simontruelove 1:0191658b6ff4 19
simontruelove 10:808cb9052f14 20 QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
simontruelove 3:4249dbdf7ed3 21
simontruelove 10:808cb9052f14 22 Timer t; //timer used in RPM
simontruelove 3:4249dbdf7ed3 23
simontruelove 10:808cb9052f14 24 PwmOut Phase1 (p23); //Pin and LED set up - originally standard pins but changed to PWM to enable speed control
simontruelove 10:808cb9052f14 25 PwmOut Phase2 (p24);
simontruelove 10:808cb9052f14 26 PwmOut Phase3 (p25);
simontruelove 10:808cb9052f14 27 PwmOut Phase4 (p26);
simontruelove 1:0191658b6ff4 28
simontruelove 10:808cb9052f14 29 AnalogOut Aout(p18); //Used with multimeter to give a speed indicator 1mV = 1RPM
simontruelove 6:f7028034aabb 30
simontruelove 10:808cb9052f14 31 //DigitalIn Button1 (p11); //not used
simontruelove 10:808cb9052f14 32 //DigitalIn Button2 (p12); //not used
simontruelove 2:3f95c82c26bb 33
simontruelove 10:808cb9052f14 34 DigitalOut led1(LED1); //LEDs used to as very basic memmory for controlling the state machines
simontruelove 2:3f95c82c26bb 35 DigitalOut led2(LED2);
simontruelove 2:3f95c82c26bb 36 DigitalOut led3(LED3);
simontruelove 2:3f95c82c26bb 37 DigitalOut led4(LED4);
simontruelove 1:0191658b6ff4 38
simontruelove 10:808cb9052f14 39 DigitalOut SerialClock (p12); //ReadKType
simontruelove 10:808cb9052f14 40 DigitalIn DOut (p13); //ReadKType
simontruelove 10:808cb9052f14 41 DigitalOut cs1 (p14); //ReadKType
simontruelove 10:808cb9052f14 42
simontruelove 10:808cb9052f14 43 int StateA = 0; //State for first 2 revolutions (calibration of the index)
simontruelove 10:808cb9052f14 44 int StateB = 0; //All state machines after calibration use this state
simontruelove 6:f7028034aabb 45 //int StateC = 0;
simontruelove 10:808cb9052f14 46 int AdjCW = 0; //CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 10:808cb9052f14 47 int AdjACW = 3; //ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 10:808cb9052f14 48 int TimePerClick = 0; //for calc of RPM
simontruelove 10:808cb9052f14 49 int TimePerRev = 0; //for calc of RPM
simontruelove 10:808cb9052f14 50 int RPS = 0; //for calc of RPMl;
simontruelove 10:808cb9052f14 51 int rpm = 0; //for calc of RPM
simontruelove 10:808cb9052f14 52 int SetPoint = 1000; //for adjusting the speed
simontruelove 10:808cb9052f14 53 int z = 80; //TimePerRev = TimePerClick * (800/z); 800 pulses per rev, PulseCount2_==80 for wheel.getwhoop_ flag. i.e. 10 points per reoluition for RPM calc.
simontruelove 10:808cb9052f14 54 int i = 0; //ReadKType
simontruelove 10:808cb9052f14 55 int Readout = 0; //ReadKType
simontruelove 1:0191658b6ff4 56
simontruelove 10:808cb9052f14 57 char c; //keyboard cotrol GetChar
simontruelove 4:3aedc9246ae4 58
simontruelove 10:808cb9052f14 59 float duty = 0.5; //velocity loop: 1 = fastest, 0.96 = slowest. Below 0.96 the motor will not operate.
simontruelove 10:808cb9052f14 60 float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
simontruelove 10:808cb9052f14 61 float x=0.1; //x=time of square wave when 1 phase energised,
simontruelove 10:808cb9052f14 62 float y=0.04; //y=time of square wave when 2 phases energised
simontruelove 10:808cb9052f14 63 float temp = 0; //ReadKType
simontruelove 1:0191658b6ff4 64
simontruelove 1:0191658b6ff4 65 int main(void)
simontruelove 3:4249dbdf7ed3 66 {
simontruelove 10:808cb9052f14 67 pc.baud(230400); //Set fastest baud rate
simontruelove 10:808cb9052f14 68 Phase1.period(0.00002); //period of 0.000002 = 2 microseconds (50kHz). Good balance of low and high speed performance.
simontruelove 9:061600a6c750 69 Phase2.period(0.00002);
simontruelove 9:061600a6c750 70 Phase3.period(0.00002);
simontruelove 9:061600a6c750 71 Phase4.period(0.00002);
simontruelove 3:4249dbdf7ed3 72 StepCW();
simontruelove 3:4249dbdf7ed3 73 Initialisation();
simontruelove 3:4249dbdf7ed3 74 wait(0.1);
simontruelove 3:4249dbdf7ed3 75 t.start();
simontruelove 10:808cb9052f14 76
simontruelove 10:808cb9052f14 77 while(wheel.getRevolutions()<2) //Index Calibration
simontruelove 6:f7028034aabb 78 {
simontruelove 5:4e5c644d5cc3 79 switch(StateA)
simontruelove 3:4249dbdf7ed3 80 {
simontruelove 6:f7028034aabb 81 case 0:Ph1();break;
simontruelove 6:f7028034aabb 82 case 1:Ph1();break;
simontruelove 6:f7028034aabb 83 case 2:Ph12();break;
simontruelove 6:f7028034aabb 84 case 3:Ph12();break;
simontruelove 6:f7028034aabb 85 case 4:Ph2();break;
simontruelove 6:f7028034aabb 86 case 5:Ph2();break;
simontruelove 6:f7028034aabb 87 case 6:Ph23();break;
simontruelove 6:f7028034aabb 88 case 7:Ph23();break;
simontruelove 6:f7028034aabb 89 case 8:Ph3();break;
simontruelove 6:f7028034aabb 90 case 9:Ph3();break;
simontruelove 6:f7028034aabb 91 case 10:Ph34();break;
simontruelove 6:f7028034aabb 92 case 11:Ph34();break;
simontruelove 6:f7028034aabb 93 case 12:Ph4();break;
simontruelove 6:f7028034aabb 94 case 13:Ph4();break;
simontruelove 6:f7028034aabb 95 case 14:Ph41();break;
simontruelove 6:f7028034aabb 96 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 97 default:break;
simontruelove 3:4249dbdf7ed3 98 }
simontruelove 3:4249dbdf7ed3 99
simontruelove 10:808cb9052f14 100 if(wheel.getYay()==1) //PulseCount_==1, yay_=1;
simontruelove 3:4249dbdf7ed3 101 {
simontruelove 5:4e5c644d5cc3 102 StateA++;
simontruelove 5:4e5c644d5cc3 103 wheel.ResetYay();
simontruelove 6:f7028034aabb 104 if (StateA>15)
simontruelove 3:4249dbdf7ed3 105 {
simontruelove 5:4e5c644d5cc3 106 StateA=0;
simontruelove 3:4249dbdf7ed3 107 }
simontruelove 3:4249dbdf7ed3 108 }
simontruelove 5:4e5c644d5cc3 109 }
simontruelove 3:4249dbdf7ed3 110
simontruelove 5:4e5c644d5cc3 111 while(1)
simontruelove 5:4e5c644d5cc3 112 {
simontruelove 10:808cb9052f14 113 while((led1 == 0) && (led2 == 0)) //If no command to operate
simontruelove 4:3aedc9246ae4 114 {
simontruelove 7:b8de1529c7fc 115 Aout = 0;
simontruelove 10:808cb9052f14 116 rpm = 0;
simontruelove 9:061600a6c750 117 Phase1.write(0);
simontruelove 9:061600a6c750 118 Phase2.write(0);
simontruelove 9:061600a6c750 119 Phase3.write(0);
simontruelove 9:061600a6c750 120 Phase4.write(0);
simontruelove 10:808cb9052f14 121 GetChar();
simontruelove 6:f7028034aabb 122 //StateB = wheel.getPulses()%16;
simontruelove 6:f7028034aabb 123 //StateC = (800+wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 6:f7028034aabb 124 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 5:4e5c644d5cc3 125 //pc.printf("0 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 126 }
simontruelove 10:808cb9052f14 127 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1)) //After Calibration, Prev CW movement, CW command
simontruelove 3:4249dbdf7ed3 128 {
simontruelove 4:3aedc9246ae4 129 GetChar();
simontruelove 6:f7028034aabb 130 StateB = (wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 7:b8de1529c7fc 131 //pc.printf("rpm = %i, Whoop = %i\n\r", rpm, wheel.getWhoop());
simontruelove 6:f7028034aabb 132 //pc.printf("StateB= %i\n\r", StateB);
simontruelove 4:3aedc9246ae4 133 //pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 134
simontruelove 4:3aedc9246ae4 135 switch(StateB)
simontruelove 4:3aedc9246ae4 136 {
simontruelove 6:f7028034aabb 137 case 0:Ph1();break;
simontruelove 6:f7028034aabb 138 case 1:Ph1();break;
simontruelove 6:f7028034aabb 139 case 2:Ph12();break;
simontruelove 6:f7028034aabb 140 case 3:Ph12();break;
simontruelove 6:f7028034aabb 141 case 4:Ph2();break;
simontruelove 6:f7028034aabb 142 case 5:Ph2();break;
simontruelove 6:f7028034aabb 143 case 6:Ph23();break;
simontruelove 6:f7028034aabb 144 case 7:Ph23();break;
simontruelove 6:f7028034aabb 145 case 8:Ph3();break;
simontruelove 6:f7028034aabb 146 case 9:Ph3();break;
simontruelove 6:f7028034aabb 147 case 10:Ph34();break;
simontruelove 6:f7028034aabb 148 case 11:Ph34();break;
simontruelove 6:f7028034aabb 149 case 12:Ph4();break;
simontruelove 6:f7028034aabb 150 case 13:Ph4();break;
simontruelove 6:f7028034aabb 151 case 14:Ph41();break;
simontruelove 6:f7028034aabb 152 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 153 default:break;
simontruelove 4:3aedc9246ae4 154 }
simontruelove 4:3aedc9246ae4 155
simontruelove 10:808cb9052f14 156 if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
simontruelove 4:3aedc9246ae4 157 {
simontruelove 5:4e5c644d5cc3 158 RPM();
simontruelove 9:061600a6c750 159 VelocityLoop();
simontruelove 4:3aedc9246ae4 160 }
simontruelove 4:3aedc9246ae4 161 }
simontruelove 4:3aedc9246ae4 162
simontruelove 10:808cb9052f14 163 while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1)) //After Calibration, Prev ACW movement, CW command
simontruelove 4:3aedc9246ae4 164 {
simontruelove 4:3aedc9246ae4 165 GetChar();
simontruelove 6:f7028034aabb 166 StateB = (800+wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 6:f7028034aabb 167 //pc.printf("StateA= %i\r", StateA);
simontruelove 4:3aedc9246ae4 168 //pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 3:4249dbdf7ed3 169
simontruelove 3:4249dbdf7ed3 170 switch(StateB)
simontruelove 3:4249dbdf7ed3 171 {
simontruelove 6:f7028034aabb 172 case 0:Ph1();break;
simontruelove 6:f7028034aabb 173 case 1:Ph1();break;
simontruelove 6:f7028034aabb 174 case 2:Ph12();break;
simontruelove 6:f7028034aabb 175 case 3:Ph12();break;
simontruelove 6:f7028034aabb 176 case 4:Ph2();break;
simontruelove 6:f7028034aabb 177 case 5:Ph2();break;
simontruelove 6:f7028034aabb 178 case 6:Ph23();break;
simontruelove 6:f7028034aabb 179 case 7:Ph23();break;
simontruelove 6:f7028034aabb 180 case 8:Ph3();break;
simontruelove 6:f7028034aabb 181 case 9:Ph3();break;
simontruelove 6:f7028034aabb 182 case 10:Ph34();break;
simontruelove 6:f7028034aabb 183 case 11:Ph34();break;
simontruelove 6:f7028034aabb 184 case 12:Ph4();break;
simontruelove 6:f7028034aabb 185 case 13:Ph4();break;
simontruelove 6:f7028034aabb 186 case 14:Ph41();break;
simontruelove 6:f7028034aabb 187 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 188 default:break;
simontruelove 3:4249dbdf7ed3 189 }
simontruelove 3:4249dbdf7ed3 190
simontruelove 10:808cb9052f14 191 if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
simontruelove 5:4e5c644d5cc3 192 {
simontruelove 5:4e5c644d5cc3 193 RPM();
simontruelove 9:061600a6c750 194 VelocityLoop();
simontruelove 5:4e5c644d5cc3 195 }
simontruelove 5:4e5c644d5cc3 196 }
simontruelove 10:808cb9052f14 197 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1)) //After Calibration, Prev CW movement, ACW command
simontruelove 4:3aedc9246ae4 198 {
simontruelove 5:4e5c644d5cc3 199 GetChar();
simontruelove 6:f7028034aabb 200 //StateB = (800+wheel.getPulses())%16;
simontruelove 6:f7028034aabb 201 StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
simontruelove 6:f7028034aabb 202 //pc.printf("StateA= %i\r", StateA);
simontruelove 5:4e5c644d5cc3 203 //pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 204 switch(StateB)
simontruelove 5:4e5c644d5cc3 205 {
simontruelove 6:f7028034aabb 206 case 15:Ph41();break;
simontruelove 6:f7028034aabb 207 case 14:Ph41();break;
simontruelove 6:f7028034aabb 208 case 13:Ph4();break;
simontruelove 6:f7028034aabb 209 case 12:Ph4();break;
simontruelove 6:f7028034aabb 210 case 11:Ph34();break;
simontruelove 6:f7028034aabb 211 case 10:Ph34();break;
simontruelove 6:f7028034aabb 212 case 9:Ph3();break;
simontruelove 6:f7028034aabb 213 case 8:Ph3();break;
simontruelove 6:f7028034aabb 214 case 7:Ph23();break;
simontruelove 6:f7028034aabb 215 case 6:Ph23();break;
simontruelove 6:f7028034aabb 216 case 5:Ph2();break;
simontruelove 6:f7028034aabb 217 case 4:Ph2();break;
simontruelove 6:f7028034aabb 218 case 3:Ph12();break;
simontruelove 6:f7028034aabb 219 case 2:Ph12();break;
simontruelove 6:f7028034aabb 220 case 1:Ph1();break;
simontruelove 6:f7028034aabb 221 case 0:Ph1();break;
simontruelove 5:4e5c644d5cc3 222 default:break;
simontruelove 5:4e5c644d5cc3 223 }
simontruelove 6:f7028034aabb 224
simontruelove 10:808cb9052f14 225 if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
simontruelove 5:4e5c644d5cc3 226 {
simontruelove 5:4e5c644d5cc3 227 RPM();
simontruelove 9:061600a6c750 228 VelocityLoop();
simontruelove 5:4e5c644d5cc3 229 }
simontruelove 3:4249dbdf7ed3 230 }
simontruelove 10:808cb9052f14 231 while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1)) //After Calibration, Prev ACW movement, ACW command
simontruelove 5:4e5c644d5cc3 232 {
simontruelove 5:4e5c644d5cc3 233 GetChar();
simontruelove 6:f7028034aabb 234 StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
simontruelove 6:f7028034aabb 235 //pc.printf("StateA= %i\r", StateA);
simontruelove 5:4e5c644d5cc3 236 //pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 237 switch(StateB)
simontruelove 5:4e5c644d5cc3 238 {
simontruelove 6:f7028034aabb 239 case 15:Ph41();break;
simontruelove 6:f7028034aabb 240 case 14:Ph41();break;
simontruelove 6:f7028034aabb 241 case 13:Ph4();break;
simontruelove 6:f7028034aabb 242 case 12:Ph4();break;
simontruelove 6:f7028034aabb 243 case 11:Ph34();break;
simontruelove 6:f7028034aabb 244 case 10:Ph34();break;
simontruelove 6:f7028034aabb 245 case 9:Ph3();break;
simontruelove 6:f7028034aabb 246 case 8:Ph3();break;
simontruelove 6:f7028034aabb 247 case 7:Ph23();break;
simontruelove 6:f7028034aabb 248 case 6:Ph23();break;
simontruelove 6:f7028034aabb 249 case 5:Ph2();break;
simontruelove 6:f7028034aabb 250 case 4:Ph2();break;
simontruelove 6:f7028034aabb 251 case 3:Ph12();break;
simontruelove 6:f7028034aabb 252 case 2:Ph12();break;
simontruelove 6:f7028034aabb 253 case 1:Ph1();break;
simontruelove 6:f7028034aabb 254 case 0:Ph1();break;
simontruelove 5:4e5c644d5cc3 255 default:break;
simontruelove 5:4e5c644d5cc3 256 }
simontruelove 10:808cb9052f14 257 if(wheel.getWhoop()==1) //PulseCount2_==80, whoop_=1;
simontruelove 5:4e5c644d5cc3 258 {
simontruelove 5:4e5c644d5cc3 259 RPM();
simontruelove 9:061600a6c750 260 VelocityLoop();
simontruelove 5:4e5c644d5cc3 261 }
simontruelove 5:4e5c644d5cc3 262 }
simontruelove 1:0191658b6ff4 263 }
simontruelove 1:0191658b6ff4 264 }
simontruelove 1:0191658b6ff4 265 void StepCW(void) //Square wave switching
simontruelove 1:0191658b6ff4 266 {
simontruelove 1:0191658b6ff4 267 Ph1();
simontruelove 3:4249dbdf7ed3 268 wait(x);
simontruelove 3:4249dbdf7ed3 269 Ph12();
simontruelove 1:0191658b6ff4 270 wait(y);
simontruelove 1:0191658b6ff4 271 Ph2();
simontruelove 3:4249dbdf7ed3 272 wait(x);
simontruelove 3:4249dbdf7ed3 273 Ph23();
simontruelove 1:0191658b6ff4 274 wait(y);
simontruelove 1:0191658b6ff4 275 Ph3();
simontruelove 3:4249dbdf7ed3 276 wait(x);
simontruelove 3:4249dbdf7ed3 277 Ph34();
simontruelove 1:0191658b6ff4 278 wait(y);
simontruelove 1:0191658b6ff4 279 Ph4();
simontruelove 3:4249dbdf7ed3 280 wait(x);
simontruelove 3:4249dbdf7ed3 281 Ph41();
simontruelove 3:4249dbdf7ed3 282 wait(y);
simontruelove 1:0191658b6ff4 283 }
simontruelove 1:0191658b6ff4 284
simontruelove 1:0191658b6ff4 285 void Ph1(void)
simontruelove 1:0191658b6ff4 286 {
simontruelove 9:061600a6c750 287 Phase1.write(duty);
simontruelove 9:061600a6c750 288 Phase2.write(0);
simontruelove 9:061600a6c750 289 Phase3.write(0);
simontruelove 9:061600a6c750 290 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 291 //wait(x);
simontruelove 3:4249dbdf7ed3 292 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 293 }
simontruelove 3:4249dbdf7ed3 294
simontruelove 3:4249dbdf7ed3 295 void Ph12 (void)
simontruelove 3:4249dbdf7ed3 296 {
simontruelove 9:061600a6c750 297 Phase1.write(duty);
simontruelove 9:061600a6c750 298 Phase2.write(duty);
simontruelove 9:061600a6c750 299 Phase3.write(0);
simontruelove 9:061600a6c750 300 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 301 //wait(y);
simontruelove 1:0191658b6ff4 302 }
simontruelove 1:0191658b6ff4 303
simontruelove 1:0191658b6ff4 304 void Ph2(void)
simontruelove 1:0191658b6ff4 305 {
simontruelove 9:061600a6c750 306 Phase1.write(0);
simontruelove 9:061600a6c750 307 Phase2.write(duty);
simontruelove 9:061600a6c750 308 Phase3.write(0);
simontruelove 9:061600a6c750 309 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 310 //wait(x);
simontruelove 3:4249dbdf7ed3 311 //pc.printf("Phase 2 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 312 }
simontruelove 3:4249dbdf7ed3 313
simontruelove 3:4249dbdf7ed3 314 void Ph23 (void)
simontruelove 3:4249dbdf7ed3 315 {
simontruelove 9:061600a6c750 316 Phase1.write(0);
simontruelove 9:061600a6c750 317 Phase2.write(duty);
simontruelove 9:061600a6c750 318 Phase3.write(duty);
simontruelove 9:061600a6c750 319 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 320 //wait(y);
simontruelove 1:0191658b6ff4 321 }
simontruelove 0:634dd505dace 322
simontruelove 1:0191658b6ff4 323 void Ph3(void)
simontruelove 1:0191658b6ff4 324 {
simontruelove 9:061600a6c750 325 Phase1.write(0);
simontruelove 9:061600a6c750 326 Phase2.write(0);
simontruelove 9:061600a6c750 327 Phase3.write(duty);
simontruelove 9:061600a6c750 328 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 329 //wait(x);
simontruelove 3:4249dbdf7ed3 330 //pc.printf("Phase 3 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 331 }
simontruelove 3:4249dbdf7ed3 332 void Ph34 (void)
simontruelove 3:4249dbdf7ed3 333 {
simontruelove 9:061600a6c750 334 Phase1.write(0);
simontruelove 9:061600a6c750 335 Phase2.write(0);
simontruelove 9:061600a6c750 336 Phase3.write(duty);
simontruelove 9:061600a6c750 337 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 338 //wait(y);
simontruelove 1:0191658b6ff4 339 }
simontruelove 1:0191658b6ff4 340
simontruelove 1:0191658b6ff4 341 void Ph4(void)
simontruelove 1:0191658b6ff4 342 {
simontruelove 9:061600a6c750 343 Phase1.write(0);
simontruelove 9:061600a6c750 344 Phase2.write(0);
simontruelove 9:061600a6c750 345 Phase3.write(0);
simontruelove 9:061600a6c750 346 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 347 //wait(x);
simontruelove 3:4249dbdf7ed3 348 //pc.printf("Phase 4 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 349 }
simontruelove 3:4249dbdf7ed3 350
simontruelove 3:4249dbdf7ed3 351 void Ph41 (void)
simontruelove 3:4249dbdf7ed3 352 {
simontruelove 9:061600a6c750 353 Phase1.write(duty);
simontruelove 9:061600a6c750 354 Phase2.write(0);
simontruelove 9:061600a6c750 355 Phase3.write(0);
simontruelove 9:061600a6c750 356 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 357 //wait(y);
simontruelove 3:4249dbdf7ed3 358 }
simontruelove 3:4249dbdf7ed3 359
simontruelove 10:808cb9052f14 360 void Initialisation (void) //Turn everything off
simontruelove 3:4249dbdf7ed3 361 {
simontruelove 9:061600a6c750 362 Phase1.write(0);
simontruelove 9:061600a6c750 363 Phase2.write(0);
simontruelove 9:061600a6c750 364 Phase3.write(0);
simontruelove 9:061600a6c750 365 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 366 led1 = 0;
simontruelove 3:4249dbdf7ed3 367 led2 = 0;
simontruelove 3:4249dbdf7ed3 368 led3 = 0;
simontruelove 3:4249dbdf7ed3 369 led4 = 0;
simontruelove 3:4249dbdf7ed3 370 wheel.ResetYay();
simontruelove 4:3aedc9246ae4 371 }
simontruelove 4:3aedc9246ae4 372
simontruelove 10:808cb9052f14 373 void GetChar (void) //read keyboard strikes with terraterm
simontruelove 4:3aedc9246ae4 374 { if (pc.readable())
simontruelove 4:3aedc9246ae4 375 {
simontruelove 4:3aedc9246ae4 376 c = pc.getc();
simontruelove 10:808cb9052f14 377 if(c == 'z') //turn on led1 causes CW operation
simontruelove 4:3aedc9246ae4 378 {
simontruelove 4:3aedc9246ae4 379 led1 = !led1;
simontruelove 4:3aedc9246ae4 380 led2 = 0;
simontruelove 10:808cb9052f14 381 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 382 }
simontruelove 10:808cb9052f14 383 if(c == 'x') //turn on led2 causes ACW operation
simontruelove 4:3aedc9246ae4 384 {
simontruelove 4:3aedc9246ae4 385 led1 = 0;
simontruelove 4:3aedc9246ae4 386 led2 = !led2 ;
simontruelove 10:808cb9052f14 387 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 388 }
simontruelove 10:808cb9052f14 389 if(c == 'q') //Increases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 390 {
simontruelove 9:061600a6c750 391 //duty = duty + 0.0001;
simontruelove 10:808cb9052f14 392 SetPoint=SetPoint+10;
simontruelove 10:808cb9052f14 393 if (SetPoint >2200)
simontruelove 10:808cb9052f14 394 {
simontruelove 10:808cb9052f14 395 SetPoint = 2200;
simontruelove 10:808cb9052f14 396 }
simontruelove 10:808cb9052f14 397 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 7:b8de1529c7fc 398 }
simontruelove 10:808cb9052f14 399 if(c == 'a') //Decreases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 400 {
simontruelove 9:061600a6c750 401 //duty = duty - 0.0001;
simontruelove 10:808cb9052f14 402 SetPoint=SetPoint-10;
simontruelove 10:808cb9052f14 403 if (SetPoint <50)
simontruelove 9:061600a6c750 404 {
simontruelove 10:808cb9052f14 405 SetPoint = 50;
simontruelove 9:061600a6c750 406 }
simontruelove 10:808cb9052f14 407 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 408 }
simontruelove 10:808cb9052f14 409 if(c== 'o')
simontruelove 10:808cb9052f14 410 {
simontruelove 10:808cb9052f14 411 AdjCW = AdjCW+1;
simontruelove 10:808cb9052f14 412 if (AdjCW >15)
simontruelove 10:808cb9052f14 413 {
simontruelove 10:808cb9052f14 414 AdjCW = 0;
simontruelove 10:808cb9052f14 415 }
simontruelove 10:808cb9052f14 416 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 417 }
simontruelove 10:808cb9052f14 418 if(c== 'k')
simontruelove 10:808cb9052f14 419 {
simontruelove 10:808cb9052f14 420 AdjCW = AdjCW-1;
simontruelove 10:808cb9052f14 421 if (AdjCW <0)
simontruelove 10:808cb9052f14 422 {
simontruelove 10:808cb9052f14 423 AdjCW = 15;
simontruelove 10:808cb9052f14 424 }
simontruelove 10:808cb9052f14 425 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 426 }
simontruelove 10:808cb9052f14 427 if(c== 'p')
simontruelove 10:808cb9052f14 428 {
simontruelove 10:808cb9052f14 429 AdjACW = AdjACW+1;
simontruelove 10:808cb9052f14 430 if (AdjACW >15)
simontruelove 10:808cb9052f14 431 {
simontruelove 10:808cb9052f14 432 AdjACW = 0;
simontruelove 10:808cb9052f14 433 }
simontruelove 10:808cb9052f14 434 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 435 }
simontruelove 10:808cb9052f14 436 if(c== 'l')
simontruelove 10:808cb9052f14 437 {
simontruelove 10:808cb9052f14 438 AdjACW = AdjACW-1;
simontruelove 10:808cb9052f14 439 if (AdjACW <0)
simontruelove 10:808cb9052f14 440 {
simontruelove 10:808cb9052f14 441 AdjACW = 15;
simontruelove 10:808cb9052f14 442 }
simontruelove 10:808cb9052f14 443 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 444 }
simontruelove 10:808cb9052f14 445 if(c=='0')
simontruelove 10:808cb9052f14 446 {
simontruelove 10:808cb9052f14 447 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 8:2e690f407ec6 448 }
simontruelove 4:3aedc9246ae4 449 }
simontruelove 5:4e5c644d5cc3 450 }
simontruelove 5:4e5c644d5cc3 451
simontruelove 5:4e5c644d5cc3 452 void RPM (void)
simontruelove 5:4e5c644d5cc3 453 {
simontruelove 10:808cb9052f14 454 wheel.ResetWhoop(); //PulseCount2_==80, whoop_=1;
simontruelove 10:808cb9052f14 455 TimePerClick = (t.read_us()); //read timer in microseconds
simontruelove 10:808cb9052f14 456 t.reset(); //reset timer
simontruelove 10:808cb9052f14 457 TimePerRev = TimePerClick * (800/z); //z = 80 (PulseCount2_==80)
simontruelove 10:808cb9052f14 458 TimePerRev = TimePerRev / 1000; //
simontruelove 5:4e5c644d5cc3 459 RPS = 10000000 / TimePerRev;
simontruelove 5:4e5c644d5cc3 460 rpm = (RPS * 60)/10000;
simontruelove 10:808cb9052f14 461 Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
simontruelove 10:808cb9052f14 462 //if(rpm < 800)
simontruelove 10:808cb9052f14 463 //{
simontruelove 10:808cb9052f14 464 // AdjCW = 0;
simontruelove 10:808cb9052f14 465 // AdjACW = 3;
simontruelove 10:808cb9052f14 466 //}
simontruelove 10:808cb9052f14 467 //if(rpm > 799 and rpm < 2000)
simontruelove 10:808cb9052f14 468 //{
simontruelove 10:808cb9052f14 469 // AdjCW = 1;
simontruelove 10:808cb9052f14 470 // AdjACW = 2;
simontruelove 10:808cb9052f14 471 //}
simontruelove 10:808cb9052f14 472 //if(rpm >1999)
simontruelove 10:808cb9052f14 473 //{
simontruelove 10:808cb9052f14 474 // AdjCW = 2;
simontruelove 10:808cb9052f14 475 // AdjACW = 1;
simontruelove 10:808cb9052f14 476 //}
simontruelove 10:808cb9052f14 477
simontruelove 10:808cb9052f14 478 //pc.printf("rpm = %i\r", rpm);
simontruelove 6:f7028034aabb 479 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 7:b8de1529c7fc 480 }
simontruelove 7:b8de1529c7fc 481
simontruelove 7:b8de1529c7fc 482 void VelocityLoop (void)
simontruelove 7:b8de1529c7fc 483 {
simontruelove 10:808cb9052f14 484 diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
simontruelove 10:808cb9052f14 485 duty = duty + (diff*0.00001); //duty is adjusted to speed up or slow down until difference = 0
simontruelove 10:808cb9052f14 486 if (duty > 1) //limits for duty. Motor will not operate below 0.96. 1 = max
simontruelove 7:b8de1529c7fc 487 {
simontruelove 9:061600a6c750 488 duty = 1;
simontruelove 7:b8de1529c7fc 489 }
simontruelove 10:808cb9052f14 490 if (duty <0.01) //3A min duty 0.96, 6.5A min duty 0.4
simontruelove 10:808cb9052f14 491 {
simontruelove 10:808cb9052f14 492 duty = 0.01;
simontruelove 7:b8de1529c7fc 493 }
simontruelove 10:808cb9052f14 494 //pc.printf("%i, %.5f, %i, %i, %i \r", SetPoint, duty, AdjCW, AdjACW, rpm); //SetPoint = %i, rpm = %i\n\r", duty, SetPoint, rpm);
simontruelove 3:4249dbdf7ed3 495 }