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@3:4249dbdf7ed3, 2018-11-14 (annotated)

Committer:
simontruelove
Date:
Wed Nov 14 09:58:13 2018 +0000
Revision:
3:4249dbdf7ed3
Parent:
2:3f95c82c26bb
Child:
4:3aedc9246ae4
2 state machines in a while (1)

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 3:4249dbdf7ed3 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 1:0191658b6ff4 14
simontruelove 1:0191658b6ff4 15 Serial pc(USBTX, USBRX); // tx, rx - set up the Terraterm input from mbed
simontruelove 1:0191658b6ff4 16
simontruelove 3:4249dbdf7ed3 17 QEI wheel(p5, p6, p8, 800, QEI::X4_ENCODING); //code for quadrature encoder see QEI.h
simontruelove 3:4249dbdf7ed3 18
simontruelove 3:4249dbdf7ed3 19 Timer t;
simontruelove 3:4249dbdf7ed3 20
simontruelove 1:0191658b6ff4 21 DigitalOut Phase1 (p21); //Pin and LED set up
simontruelove 1:0191658b6ff4 22 DigitalOut Phase2 (p22);
simontruelove 1:0191658b6ff4 23 DigitalOut Phase3 (p23);
simontruelove 1:0191658b6ff4 24 DigitalOut Phase4 (p24);
simontruelove 1:0191658b6ff4 25
simontruelove 2:3f95c82c26bb 26 DigitalIn Button1 (p11);
simontruelove 2:3f95c82c26bb 27
simontruelove 2:3f95c82c26bb 28 DigitalOut led1(LED1);
simontruelove 2:3f95c82c26bb 29 DigitalOut led2(LED2);
simontruelove 2:3f95c82c26bb 30 DigitalOut led3(LED3);
simontruelove 2:3f95c82c26bb 31 DigitalOut led4(LED4);
simontruelove 1:0191658b6ff4 32
simontruelove 3:4249dbdf7ed3 33 int StateA = 0;
simontruelove 3:4249dbdf7ed3 34 int StateB = 0;
simontruelove 3:4249dbdf7ed3 35 int TimePerClick = 0;
simontruelove 3:4249dbdf7ed3 36 int TimePerRev = 0;
simontruelove 3:4249dbdf7ed3 37 int RPS = 0;
simontruelove 3:4249dbdf7ed3 38 int RPM = 0;
simontruelove 1:0191658b6ff4 39
simontruelove 3:4249dbdf7ed3 40 float x=0.05, y=0.02, z=1; //x=time of square wave when 1 phase energised, y=time of square wave when 2 phases energised
simontruelove 1:0191658b6ff4 41
simontruelove 1:0191658b6ff4 42 int main(void)
simontruelove 3:4249dbdf7ed3 43 {
simontruelove 1:0191658b6ff4 44 pc.baud(230400); //Set fastest baud rate
simontruelove 3:4249dbdf7ed3 45 StepCW();
simontruelove 3:4249dbdf7ed3 46 Initialisation();
simontruelove 3:4249dbdf7ed3 47 wait(0.1);
simontruelove 3:4249dbdf7ed3 48 t.start();
simontruelove 3:4249dbdf7ed3 49
simontruelove 3:4249dbdf7ed3 50 while(wheel.getRevolutions()==0)
simontruelove 3:4249dbdf7ed3 51 {
simontruelove 3:4249dbdf7ed3 52 switch(StateA)
simontruelove 3:4249dbdf7ed3 53 {
simontruelove 3:4249dbdf7ed3 54 case 0:Ph1();pc.printf("Ph1= %i\n\r", wheel.getPulses());break;
simontruelove 3:4249dbdf7ed3 55 case 1:Ph12();pc.printf("Ph12= %i\n\r", wheel.getPulses());break;
simontruelove 3:4249dbdf7ed3 56 case 2:Ph2();pc.printf("Ph2= %i\n\r", wheel.getPulses());break;
simontruelove 3:4249dbdf7ed3 57 case 3:Ph23();pc.printf("Ph23= %i\n\r", wheel.getPulses());break;
simontruelove 3:4249dbdf7ed3 58 case 4:Ph3();pc.printf("Ph3= %i\n\r", wheel.getPulses());break;
simontruelove 3:4249dbdf7ed3 59 case 5:Ph34();pc.printf("Ph34= %i\n\r", wheel.getPulses());break;
simontruelove 3:4249dbdf7ed3 60 case 6:Ph4();pc.printf("Ph4= %i\n\r", wheel.getPulses());break;
simontruelove 3:4249dbdf7ed3 61 case 7:Ph41();pc.printf("Ph41= %i\n\r", wheel.getPulses());break;
simontruelove 3:4249dbdf7ed3 62 default:break;
simontruelove 3:4249dbdf7ed3 63 }
simontruelove 3:4249dbdf7ed3 64
simontruelove 3:4249dbdf7ed3 65 if(wheel.getYay()==1)
simontruelove 3:4249dbdf7ed3 66 {
simontruelove 3:4249dbdf7ed3 67 StateA++;
simontruelove 3:4249dbdf7ed3 68 wheel.ResetYay();
simontruelove 3:4249dbdf7ed3 69 if (StateA>7)
simontruelove 3:4249dbdf7ed3 70 {
simontruelove 3:4249dbdf7ed3 71 StateA=0;
simontruelove 3:4249dbdf7ed3 72 }
simontruelove 3:4249dbdf7ed3 73 }
simontruelove 1:0191658b6ff4 74 }
simontruelove 3:4249dbdf7ed3 75
simontruelove 3:4249dbdf7ed3 76 while(1)
simontruelove 3:4249dbdf7ed3 77 {
simontruelove 3:4249dbdf7ed3 78 while(wheel.getRevolutions()>0 && wheel.getPulses()>0)
simontruelove 3:4249dbdf7ed3 79 {
simontruelove 3:4249dbdf7ed3 80 StateB = (wheel.getPulses())%16;
simontruelove 3:4249dbdf7ed3 81 //pc.printf("StateA= %i\n\r", StateA);
simontruelove 3:4249dbdf7ed3 82 pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 3:4249dbdf7ed3 83 //pc.printf("",);
simontruelove 3:4249dbdf7ed3 84 // pc.printf("",);
simontruelove 3:4249dbdf7ed3 85
simontruelove 3:4249dbdf7ed3 86 switch(StateB)
simontruelove 3:4249dbdf7ed3 87 {
simontruelove 3:4249dbdf7ed3 88 case 0:Ph3();break;
simontruelove 3:4249dbdf7ed3 89 case 1:Ph3();break;
simontruelove 3:4249dbdf7ed3 90 case 2:Ph34();break;
simontruelove 3:4249dbdf7ed3 91 case 3:Ph34();break;
simontruelove 3:4249dbdf7ed3 92 case 4:Ph4();break;
simontruelove 3:4249dbdf7ed3 93 case 5:Ph4();break;
simontruelove 3:4249dbdf7ed3 94 case 6:Ph41();break;
simontruelove 3:4249dbdf7ed3 95 case 7:Ph41();break;
simontruelove 3:4249dbdf7ed3 96 case 8:Ph1();break;
simontruelove 3:4249dbdf7ed3 97 case 9:Ph1();break;
simontruelove 3:4249dbdf7ed3 98 case 10:Ph12();break;
simontruelove 3:4249dbdf7ed3 99 case 11:Ph12();break;
simontruelove 3:4249dbdf7ed3 100 case 12:Ph2();break;
simontruelove 3:4249dbdf7ed3 101 case 13:Ph2();break;
simontruelove 3:4249dbdf7ed3 102 case 14:Ph23();break;
simontruelove 3:4249dbdf7ed3 103 case 15:Ph23();break;
simontruelove 3:4249dbdf7ed3 104 default:break;
simontruelove 3:4249dbdf7ed3 105 }
simontruelove 3:4249dbdf7ed3 106
simontruelove 3:4249dbdf7ed3 107 if(wheel.getYay()==1)
simontruelove 3:4249dbdf7ed3 108 {
simontruelove 3:4249dbdf7ed3 109 wheel.ResetYay();
simontruelove 3:4249dbdf7ed3 110 TimePerClick = (t.read_us());
simontruelove 3:4249dbdf7ed3 111 t.reset();
simontruelove 3:4249dbdf7ed3 112 TimePerRev = TimePerClick * 400;
simontruelove 3:4249dbdf7ed3 113 TimePerRev = TimePerRev / 1000;
simontruelove 3:4249dbdf7ed3 114 RPS = 10000000 / TimePerRev;
simontruelove 3:4249dbdf7ed3 115 RPM = (RPS * 60)/10000;
simontruelove 3:4249dbdf7ed3 116 //pc.printf("RPM = %d\n\r", RPM);
simontruelove 3:4249dbdf7ed3 117 }
simontruelove 3:4249dbdf7ed3 118 }
simontruelove 3:4249dbdf7ed3 119
simontruelove 3:4249dbdf7ed3 120
simontruelove 3:4249dbdf7ed3 121 while(wheel.getRevolutions()>0 && wheel.getPulses()<1)
simontruelove 3:4249dbdf7ed3 122 {
simontruelove 3:4249dbdf7ed3 123 StateB = (800+wheel.getPulses())%16;
simontruelove 3:4249dbdf7ed3 124 //pc.printf("StateA= %i\n\r", StateA);
simontruelove 3:4249dbdf7ed3 125 pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 3:4249dbdf7ed3 126 //pc.printf("",);
simontruelove 3:4249dbdf7ed3 127 // pc.printf("",);
simontruelove 3:4249dbdf7ed3 128
simontruelove 3:4249dbdf7ed3 129 switch(StateB)
simontruelove 3:4249dbdf7ed3 130 {
simontruelove 3:4249dbdf7ed3 131 case 0:Ph3();break;
simontruelove 3:4249dbdf7ed3 132 case 1:Ph3();break;
simontruelove 3:4249dbdf7ed3 133 case 2:Ph34();break;
simontruelove 3:4249dbdf7ed3 134 case 3:Ph34();break;
simontruelove 3:4249dbdf7ed3 135 case 4:Ph4();break;
simontruelove 3:4249dbdf7ed3 136 case 5:Ph4();break;
simontruelove 3:4249dbdf7ed3 137 case 6:Ph41();break;
simontruelove 3:4249dbdf7ed3 138 case 7:Ph41();break;
simontruelove 3:4249dbdf7ed3 139 case 8:Ph1();break;
simontruelove 3:4249dbdf7ed3 140 case 9:Ph1();break;
simontruelove 3:4249dbdf7ed3 141 case 10:Ph12();break;
simontruelove 3:4249dbdf7ed3 142 case 11:Ph12();break;
simontruelove 3:4249dbdf7ed3 143 case 12:Ph2();break;
simontruelove 3:4249dbdf7ed3 144 case 13:Ph2();break;
simontruelove 3:4249dbdf7ed3 145 case 14:Ph23();break;
simontruelove 3:4249dbdf7ed3 146 case 15:Ph23();break;
simontruelove 3:4249dbdf7ed3 147 default:break;
simontruelove 3:4249dbdf7ed3 148 }
simontruelove 3:4249dbdf7ed3 149
simontruelove 3:4249dbdf7ed3 150 if(wheel.getYay()==1)
simontruelove 3:4249dbdf7ed3 151 {
simontruelove 3:4249dbdf7ed3 152 wheel.ResetYay();
simontruelove 3:4249dbdf7ed3 153 TimePerClick = (t.read_us());
simontruelove 3:4249dbdf7ed3 154 t.reset();
simontruelove 3:4249dbdf7ed3 155 TimePerRev = TimePerClick * 400;
simontruelove 3:4249dbdf7ed3 156 TimePerRev = TimePerRev / 1000;
simontruelove 3:4249dbdf7ed3 157 RPS = 10000000 / TimePerRev;
simontruelove 3:4249dbdf7ed3 158 RPM = (RPS * 60)/10000;
simontruelove 3:4249dbdf7ed3 159 //pc.printf("RPM = %d\n\r", RPM);
simontruelove 3:4249dbdf7ed3 160 }
simontruelove 3:4249dbdf7ed3 161 }
simontruelove 3:4249dbdf7ed3 162
simontruelove 1:0191658b6ff4 163 }
simontruelove 1:0191658b6ff4 164 }
simontruelove 1:0191658b6ff4 165
simontruelove 1:0191658b6ff4 166 void StepCW(void) //Square wave switching
simontruelove 1:0191658b6ff4 167 {
simontruelove 1:0191658b6ff4 168 Ph1();
simontruelove 3:4249dbdf7ed3 169 wait(x);
simontruelove 3:4249dbdf7ed3 170 Ph12();
simontruelove 1:0191658b6ff4 171 wait(y);
simontruelove 1:0191658b6ff4 172 Ph2();
simontruelove 3:4249dbdf7ed3 173 wait(x);
simontruelove 3:4249dbdf7ed3 174 Ph23();
simontruelove 1:0191658b6ff4 175 wait(y);
simontruelove 1:0191658b6ff4 176 Ph3();
simontruelove 3:4249dbdf7ed3 177 wait(x);
simontruelove 3:4249dbdf7ed3 178 Ph34();
simontruelove 1:0191658b6ff4 179 wait(y);
simontruelove 1:0191658b6ff4 180 Ph4();
simontruelove 3:4249dbdf7ed3 181 wait(x);
simontruelove 3:4249dbdf7ed3 182 Ph41();
simontruelove 3:4249dbdf7ed3 183 wait(y);
simontruelove 1:0191658b6ff4 184 }
simontruelove 1:0191658b6ff4 185
simontruelove 1:0191658b6ff4 186 void Ph1(void)
simontruelove 1:0191658b6ff4 187 {
simontruelove 3:4249dbdf7ed3 188 Phase1 = 1;
simontruelove 3:4249dbdf7ed3 189 Phase2 = Phase3 = Phase4 = 0;
simontruelove 3:4249dbdf7ed3 190 //wait(x);
simontruelove 3:4249dbdf7ed3 191 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 192 }
simontruelove 3:4249dbdf7ed3 193
simontruelove 3:4249dbdf7ed3 194 void Ph12 (void)
simontruelove 3:4249dbdf7ed3 195 {
simontruelove 3:4249dbdf7ed3 196 Phase1 = Phase2 = 1;
simontruelove 3:4249dbdf7ed3 197 Phase3 = Phase4 = 0;
simontruelove 3:4249dbdf7ed3 198 //wait(y);
simontruelove 1:0191658b6ff4 199 }
simontruelove 1:0191658b6ff4 200
simontruelove 1:0191658b6ff4 201 void Ph2(void)
simontruelove 1:0191658b6ff4 202 {
simontruelove 1:0191658b6ff4 203 Phase2 = 1;
simontruelove 1:0191658b6ff4 204 Phase1 = Phase3 = Phase4 = 0;
simontruelove 3:4249dbdf7ed3 205 //wait(x);
simontruelove 3:4249dbdf7ed3 206 //pc.printf("Phase 2 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 207 }
simontruelove 3:4249dbdf7ed3 208
simontruelove 3:4249dbdf7ed3 209 void Ph23 (void)
simontruelove 3:4249dbdf7ed3 210 {
simontruelove 3:4249dbdf7ed3 211 Phase2 = Phase3 = 1;
simontruelove 3:4249dbdf7ed3 212 Phase4 = Phase1 = 0;
simontruelove 3:4249dbdf7ed3 213 //wait(y);
simontruelove 1:0191658b6ff4 214 }
simontruelove 0:634dd505dace 215
simontruelove 1:0191658b6ff4 216 void Ph3(void)
simontruelove 1:0191658b6ff4 217 {
simontruelove 1:0191658b6ff4 218 Phase3 = 1;
simontruelove 1:0191658b6ff4 219 Phase1 = Phase2 = Phase4 = 0;
simontruelove 3:4249dbdf7ed3 220 //wait(x);
simontruelove 3:4249dbdf7ed3 221 //pc.printf("Phase 3 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 222 }
simontruelove 3:4249dbdf7ed3 223 void Ph34 (void)
simontruelove 3:4249dbdf7ed3 224 {
simontruelove 3:4249dbdf7ed3 225 Phase3 = Phase4 = 1;
simontruelove 3:4249dbdf7ed3 226 Phase1 = Phase2 = 0;
simontruelove 3:4249dbdf7ed3 227 //wait(y);
simontruelove 1:0191658b6ff4 228 }
simontruelove 1:0191658b6ff4 229
simontruelove 1:0191658b6ff4 230 void Ph4(void)
simontruelove 1:0191658b6ff4 231 {
simontruelove 1:0191658b6ff4 232 Phase4 = 1;
simontruelove 1:0191658b6ff4 233 Phase1 = Phase2 = Phase3 = 0;
simontruelove 3:4249dbdf7ed3 234 //wait(x);
simontruelove 3:4249dbdf7ed3 235 //pc.printf("Phase 4 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 236 }
simontruelove 3:4249dbdf7ed3 237
simontruelove 3:4249dbdf7ed3 238 void Ph41 (void)
simontruelove 3:4249dbdf7ed3 239 {
simontruelove 3:4249dbdf7ed3 240 Phase4 = Phase1 = 1;
simontruelove 3:4249dbdf7ed3 241 Phase2 = Phase3 = 0;
simontruelove 3:4249dbdf7ed3 242 //wait(y);
simontruelove 3:4249dbdf7ed3 243 }
simontruelove 3:4249dbdf7ed3 244
simontruelove 3:4249dbdf7ed3 245 void Initialisation (void)
simontruelove 3:4249dbdf7ed3 246 {
simontruelove 3:4249dbdf7ed3 247 Phase1 = 0;
simontruelove 3:4249dbdf7ed3 248 Phase2 = 0;
simontruelove 3:4249dbdf7ed3 249 Phase3 = 0;
simontruelove 3:4249dbdf7ed3 250 Phase4 = 0;
simontruelove 3:4249dbdf7ed3 251 led1 = 0;
simontruelove 3:4249dbdf7ed3 252 led2 = 0;
simontruelove 3:4249dbdf7ed3 253 led3 = 0;
simontruelove 3:4249dbdf7ed3 254 led4 = 0;
simontruelove 3:4249dbdf7ed3 255 wheel.ResetYay();
simontruelove 3:4249dbdf7ed3 256
simontruelove 3:4249dbdf7ed3 257
simontruelove 3:4249dbdf7ed3 258 }