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@13:da9d3fbbe407, 2019-01-10 (annotated)

Committer:
simontruelove
Date:
Thu Jan 10 15:07:19 2019 +0000
Revision:
13:da9d3fbbe407
Parent:
12:cbea987a3ec4
Child:
14:1eb49362a607
Auto phase adjust, no handover

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 12:cbea987a3ec4 28 PwmOut Phase3 (p23);
simontruelove 12:cbea987a3ec4 29 PwmOut Phase4 (p24);
simontruelove 1:0191658b6ff4 30
simontruelove 12:cbea987a3ec4 31 //AnalogOut Aout(p18); //Used with multimeter to give a speed indicator 1mV = 1RPM
simontruelove 6:f7028034aabb 32
simontruelove 10:808cb9052f14 33 //DigitalIn Button1 (p11); //not used
simontruelove 10:808cb9052f14 34 //DigitalIn Button2 (p12); //not used
simontruelove 2:3f95c82c26bb 35
simontruelove 10:808cb9052f14 36 DigitalOut led1(LED1); //LEDs used to as very basic memmory for controlling the state machines
simontruelove 2:3f95c82c26bb 37 DigitalOut led2(LED2);
simontruelove 2:3f95c82c26bb 38 DigitalOut led3(LED3);
simontruelove 2:3f95c82c26bb 39 DigitalOut led4(LED4);
simontruelove 12:cbea987a3ec4 40 DigitalOut UnUsedPhase1 (p25);
simontruelove 12:cbea987a3ec4 41 DigitalOut UnUsedPhase2 (p26);
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 12:cbea987a3ec4 49 int AdjCW = 2; //CW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 12:cbea987a3ec4 50 int AdjACW = 5; //ACW offset to adjust phase firing to give the fastest speed = used to calculate stateB
simontruelove 10:808cb9052f14 51 int TimePerClick = 0; //for calc of RPM
simontruelove 10:808cb9052f14 52 int TimePerRev = 0; //for calc of RPM
simontruelove 12:cbea987a3ec4 53 int RPS = 0; //for calc of RPM
simontruelove 10:808cb9052f14 54 int rpm = 0; //for calc of RPM
simontruelove 13:da9d3fbbe407 55 int SetPoint = 700; //for adjusting the speed
simontruelove 12:cbea987a3ec4 56 int z = 800; //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 57 int T = 80; //Motor temp limit
simontruelove 11:74eeb8871fe6 58
simontruelove 10:808cb9052f14 59 char c; //keyboard cotrol GetChar
simontruelove 4:3aedc9246ae4 60
simontruelove 13:da9d3fbbe407 61 float duty = 1; //velocity loop: 1 = fastest, 0.96 = slowest. Below 0.96 the motor will not operate.
simontruelove 10:808cb9052f14 62 float diff = 0.0; //Velocity loop: diff = SetPoint - rpm;
simontruelove 10:808cb9052f14 63 float x=0.1; //x=time of square wave when 1 phase energised,
simontruelove 10:808cb9052f14 64 float y=0.04; //y=time of square wave when 2 phases energised
simontruelove 10:808cb9052f14 65 float temp = 0; //ReadKType
simontruelove 1:0191658b6ff4 66
simontruelove 1:0191658b6ff4 67 int main(void)
simontruelove 3:4249dbdf7ed3 68 {
simontruelove 10:808cb9052f14 69 pc.baud(230400); //Set fastest baud rate
simontruelove 10:808cb9052f14 70 Phase1.period(0.00002); //period of 0.000002 = 2 microseconds (50kHz). Good balance of low and high speed performance.
simontruelove 9:061600a6c750 71 Phase2.period(0.00002);
simontruelove 9:061600a6c750 72 Phase3.period(0.00002);
simontruelove 9:061600a6c750 73 Phase4.period(0.00002);
simontruelove 3:4249dbdf7ed3 74 StepCW();
simontruelove 3:4249dbdf7ed3 75 Initialisation();
simontruelove 3:4249dbdf7ed3 76 wait(0.1);
simontruelove 3:4249dbdf7ed3 77 t.start();
simontruelove 12:cbea987a3ec4 78 SerialClock = 0;
simontruelove 12:cbea987a3ec4 79
simontruelove 10:808cb9052f14 80 while(wheel.getRevolutions()<2) //Index Calibration
simontruelove 6:f7028034aabb 81 {
simontruelove 5:4e5c644d5cc3 82 switch(StateA)
simontruelove 3:4249dbdf7ed3 83 {
simontruelove 6:f7028034aabb 84 case 0:Ph1();break;
simontruelove 6:f7028034aabb 85 case 1:Ph1();break;
simontruelove 6:f7028034aabb 86 case 2:Ph12();break;
simontruelove 6:f7028034aabb 87 case 3:Ph12();break;
simontruelove 6:f7028034aabb 88 case 4:Ph2();break;
simontruelove 6:f7028034aabb 89 case 5:Ph2();break;
simontruelove 6:f7028034aabb 90 case 6:Ph23();break;
simontruelove 6:f7028034aabb 91 case 7:Ph23();break;
simontruelove 6:f7028034aabb 92 case 8:Ph3();break;
simontruelove 6:f7028034aabb 93 case 9:Ph3();break;
simontruelove 6:f7028034aabb 94 case 10:Ph34();break;
simontruelove 6:f7028034aabb 95 case 11:Ph34();break;
simontruelove 6:f7028034aabb 96 case 12:Ph4();break;
simontruelove 6:f7028034aabb 97 case 13:Ph4();break;
simontruelove 6:f7028034aabb 98 case 14:Ph41();break;
simontruelove 6:f7028034aabb 99 case 15:Ph41();break;
simontruelove 5:4e5c644d5cc3 100 default:break;
simontruelove 3:4249dbdf7ed3 101 }
simontruelove 3:4249dbdf7ed3 102
simontruelove 10:808cb9052f14 103 if(wheel.getYay()==1) //PulseCount_==1, yay_=1;
simontruelove 3:4249dbdf7ed3 104 {
simontruelove 5:4e5c644d5cc3 105 StateA++;
simontruelove 5:4e5c644d5cc3 106 wheel.ResetYay();
simontruelove 6:f7028034aabb 107 if (StateA>15)
simontruelove 3:4249dbdf7ed3 108 {
simontruelove 5:4e5c644d5cc3 109 StateA=0;
simontruelove 3:4249dbdf7ed3 110 }
simontruelove 3:4249dbdf7ed3 111 }
simontruelove 5:4e5c644d5cc3 112 }
simontruelove 3:4249dbdf7ed3 113
simontruelove 5:4e5c644d5cc3 114 while(1)
simontruelove 5:4e5c644d5cc3 115 {
simontruelove 10:808cb9052f14 116 while((led1 == 0) && (led2 == 0)) //If no command to operate
simontruelove 4:3aedc9246ae4 117 {
simontruelove 12:cbea987a3ec4 118 //Aout = 0;
simontruelove 10:808cb9052f14 119 rpm = 0;
simontruelove 9:061600a6c750 120 Phase1.write(0);
simontruelove 9:061600a6c750 121 Phase2.write(0);
simontruelove 9:061600a6c750 122 Phase3.write(0);
simontruelove 9:061600a6c750 123 Phase4.write(0);
simontruelove 10:808cb9052f14 124 GetChar();
simontruelove 11:74eeb8871fe6 125 ReadKType();
simontruelove 6:f7028034aabb 126 //StateB = wheel.getPulses()%16;
simontruelove 6:f7028034aabb 127 //StateC = (800+wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 6:f7028034aabb 128 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 5:4e5c644d5cc3 129 //pc.printf("0 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 130 }
simontruelove 11:74eeb8871fe6 131 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led1==1) && (temp<T)) //After Calibration, Prev CW movement, CW command
simontruelove 3:4249dbdf7ed3 132 {
simontruelove 4:3aedc9246ae4 133 GetChar();
simontruelove 6:f7028034aabb 134 StateB = (wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 7:b8de1529c7fc 135 //pc.printf("rpm = %i, Whoop = %i\n\r", rpm, wheel.getWhoop());
simontruelove 6:f7028034aabb 136 //pc.printf("StateB= %i\n\r", StateB);
simontruelove 4:3aedc9246ae4 137 //pc.printf("1 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 4:3aedc9246ae4 138
simontruelove 4:3aedc9246ae4 139 switch(StateB)
simontruelove 4:3aedc9246ae4 140 {
simontruelove 6:f7028034aabb 141 case 0:Ph1();break;
simontruelove 6:f7028034aabb 142 case 1:Ph1();break;
simontruelove 13:da9d3fbbe407 143 case 2:Ph1();break;
simontruelove 13:da9d3fbbe407 144 case 3:Ph1();break;
simontruelove 6:f7028034aabb 145 case 4:Ph2();break;
simontruelove 6:f7028034aabb 146 case 5:Ph2();break;
simontruelove 13:da9d3fbbe407 147 case 6:Ph2();break;
simontruelove 13:da9d3fbbe407 148 case 7:Ph2();break;
simontruelove 6:f7028034aabb 149 case 8:Ph3();break;
simontruelove 6:f7028034aabb 150 case 9:Ph3();break;
simontruelove 13:da9d3fbbe407 151 case 10:Ph3();break;
simontruelove 13:da9d3fbbe407 152 case 11:Ph3();break;
simontruelove 6:f7028034aabb 153 case 12:Ph4();break;
simontruelove 6:f7028034aabb 154 case 13:Ph4();break;
simontruelove 13:da9d3fbbe407 155 case 14:Ph4();break;
simontruelove 13:da9d3fbbe407 156 case 15:Ph4();break;
simontruelove 5:4e5c644d5cc3 157 default:break;
simontruelove 4:3aedc9246ae4 158 }
simontruelove 4:3aedc9246ae4 159
simontruelove 12:cbea987a3ec4 160 if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
simontruelove 4:3aedc9246ae4 161 {
simontruelove 5:4e5c644d5cc3 162 RPM();
simontruelove 9:061600a6c750 163 VelocityLoop();
simontruelove 11:74eeb8871fe6 164 }
simontruelove 4:3aedc9246ae4 165 }
simontruelove 4:3aedc9246ae4 166
simontruelove 11:74eeb8871fe6 167 while(wheel.getRevolutions()>1 && wheel.getPulses()<1 && (led1==1) && (temp<T)) //After Calibration, Prev ACW movement, CW command
simontruelove 4:3aedc9246ae4 168 {
simontruelove 4:3aedc9246ae4 169 GetChar();
simontruelove 6:f7028034aabb 170 StateB = (800+wheel.getPulses()+StateA+AdjCW)%16;
simontruelove 6:f7028034aabb 171 //pc.printf("StateA= %i\r", StateA);
simontruelove 4:3aedc9246ae4 172 //pc.printf("2 StateB= %i, Pulses= %i, Revs= %i\r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 3:4249dbdf7ed3 173
simontruelove 3:4249dbdf7ed3 174 switch(StateB)
simontruelove 3:4249dbdf7ed3 175 {
simontruelove 6:f7028034aabb 176 case 0:Ph1();break;
simontruelove 6:f7028034aabb 177 case 1:Ph1();break;
simontruelove 13:da9d3fbbe407 178 case 2:Ph1();break;
simontruelove 13:da9d3fbbe407 179 case 3:Ph1();break;
simontruelove 6:f7028034aabb 180 case 4:Ph2();break;
simontruelove 6:f7028034aabb 181 case 5:Ph2();break;
simontruelove 13:da9d3fbbe407 182 case 6:Ph2();break;
simontruelove 13:da9d3fbbe407 183 case 7:Ph2();break;
simontruelove 6:f7028034aabb 184 case 8:Ph3();break;
simontruelove 6:f7028034aabb 185 case 9:Ph3();break;
simontruelove 13:da9d3fbbe407 186 case 10:Ph3();break;
simontruelove 13:da9d3fbbe407 187 case 11:Ph3();break;
simontruelove 6:f7028034aabb 188 case 12:Ph4();break;
simontruelove 6:f7028034aabb 189 case 13:Ph4();break;
simontruelove 13:da9d3fbbe407 190 case 14:Ph4();break;
simontruelove 13:da9d3fbbe407 191 case 15:Ph4();break;
simontruelove 5:4e5c644d5cc3 192 default:break;
simontruelove 3:4249dbdf7ed3 193 }
simontruelove 3:4249dbdf7ed3 194
simontruelove 12:cbea987a3ec4 195 if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
simontruelove 5:4e5c644d5cc3 196 {
simontruelove 5:4e5c644d5cc3 197 RPM();
simontruelove 9:061600a6c750 198 VelocityLoop();
simontruelove 11:74eeb8871fe6 199 }
simontruelove 5:4e5c644d5cc3 200 }
simontruelove 11:74eeb8871fe6 201 while((wheel.getRevolutions()>1) && (wheel.getPulses()>0) && (led2==1) && (temp<T)) //After Calibration, Prev CW movement, ACW command
simontruelove 4:3aedc9246ae4 202 {
simontruelove 5:4e5c644d5cc3 203 GetChar();
simontruelove 6:f7028034aabb 204 //StateB = (800+wheel.getPulses())%16;
simontruelove 6:f7028034aabb 205 StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
simontruelove 6:f7028034aabb 206 //pc.printf("StateA= %i\r", StateA);
simontruelove 5:4e5c644d5cc3 207 //pc.printf("3 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 208 switch(StateB)
simontruelove 5:4e5c644d5cc3 209 {
simontruelove 13:da9d3fbbe407 210 case 15:Ph4();break;
simontruelove 13:da9d3fbbe407 211 case 14:Ph4();break;
simontruelove 6:f7028034aabb 212 case 13:Ph4();break;
simontruelove 6:f7028034aabb 213 case 12:Ph4();break;
simontruelove 13:da9d3fbbe407 214 case 11:Ph3();break;
simontruelove 13:da9d3fbbe407 215 case 10:Ph3();break;
simontruelove 6:f7028034aabb 216 case 9:Ph3();break;
simontruelove 6:f7028034aabb 217 case 8:Ph3();break;
simontruelove 13:da9d3fbbe407 218 case 7:Ph2();break;
simontruelove 13:da9d3fbbe407 219 case 6:Ph2();break;
simontruelove 6:f7028034aabb 220 case 5:Ph2();break;
simontruelove 6:f7028034aabb 221 case 4:Ph2();break;
simontruelove 13:da9d3fbbe407 222 case 3:Ph1();break;
simontruelove 13:da9d3fbbe407 223 case 2:Ph1();break;
simontruelove 6:f7028034aabb 224 case 1:Ph1();break;
simontruelove 6:f7028034aabb 225 case 0:Ph1();break;
simontruelove 5:4e5c644d5cc3 226 default:break;
simontruelove 5:4e5c644d5cc3 227 }
simontruelove 6:f7028034aabb 228
simontruelove 12:cbea987a3ec4 229 if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
simontruelove 5:4e5c644d5cc3 230 {
simontruelove 5:4e5c644d5cc3 231 RPM();
simontruelove 9:061600a6c750 232 VelocityLoop();
simontruelove 5:4e5c644d5cc3 233 }
simontruelove 3:4249dbdf7ed3 234 }
simontruelove 11:74eeb8871fe6 235 while((wheel.getRevolutions()>1) && (wheel.getPulses()<1) && (led2==1) &&(temp<T)) //After Calibration, Prev ACW movement, ACW command
simontruelove 5:4e5c644d5cc3 236 {
simontruelove 5:4e5c644d5cc3 237 GetChar();
simontruelove 6:f7028034aabb 238 StateB = (800+wheel.getPulses()+StateA+AdjACW)%16;
simontruelove 6:f7028034aabb 239 //pc.printf("StateA= %i\r", StateA);
simontruelove 5:4e5c644d5cc3 240 //pc.printf("4 StateB= %i, Pulses= %i, Revs= %i \r", StateB,wheel.getPulses(),wheel.getRevolutions());
simontruelove 5:4e5c644d5cc3 241 switch(StateB)
simontruelove 5:4e5c644d5cc3 242 {
simontruelove 13:da9d3fbbe407 243 case 15:Ph4();break;
simontruelove 13:da9d3fbbe407 244 case 14:Ph4();break;
simontruelove 6:f7028034aabb 245 case 13:Ph4();break;
simontruelove 6:f7028034aabb 246 case 12:Ph4();break;
simontruelove 13:da9d3fbbe407 247 case 11:Ph3();break;
simontruelove 13:da9d3fbbe407 248 case 10:Ph3();break;
simontruelove 6:f7028034aabb 249 case 9:Ph3();break;
simontruelove 6:f7028034aabb 250 case 8:Ph3();break;
simontruelove 13:da9d3fbbe407 251 case 7:Ph2();break;
simontruelove 13:da9d3fbbe407 252 case 6:Ph2();break;
simontruelove 6:f7028034aabb 253 case 5:Ph2();break;
simontruelove 6:f7028034aabb 254 case 4:Ph2();break;
simontruelove 13:da9d3fbbe407 255 case 3:Ph1();break;
simontruelove 13:da9d3fbbe407 256 case 2:Ph1();break;
simontruelove 6:f7028034aabb 257 case 1:Ph1();break;
simontruelove 6:f7028034aabb 258 case 0:Ph1();break;
simontruelove 5:4e5c644d5cc3 259 default:break;
simontruelove 5:4e5c644d5cc3 260 }
simontruelove 12:cbea987a3ec4 261 if(wheel.getWhoop()==1) //PulseCount2_==800, whoop_=1;
simontruelove 5:4e5c644d5cc3 262 {
simontruelove 5:4e5c644d5cc3 263 RPM();
simontruelove 9:061600a6c750 264 VelocityLoop();
simontruelove 11:74eeb8871fe6 265 }
simontruelove 5:4e5c644d5cc3 266 }
simontruelove 11:74eeb8871fe6 267 while(temp>(T-1))
simontruelove 11:74eeb8871fe6 268 {
simontruelove 11:74eeb8871fe6 269 Initialisation();
simontruelove 11:74eeb8871fe6 270 pc.printf("Motor Over Temp\n\r");
simontruelove 11:74eeb8871fe6 271 while(1)
simontruelove 11:74eeb8871fe6 272 {
simontruelove 11:74eeb8871fe6 273 ReadKType();
simontruelove 11:74eeb8871fe6 274 pc.printf("%f\r",temp);
simontruelove 11:74eeb8871fe6 275 wait(1);
simontruelove 11:74eeb8871fe6 276 if(temp<T-20)
simontruelove 11:74eeb8871fe6 277 {
simontruelove 11:74eeb8871fe6 278 pc.printf("Motor Back Online\n\r");
simontruelove 11:74eeb8871fe6 279 break;
simontruelove 11:74eeb8871fe6 280 }
simontruelove 11:74eeb8871fe6 281 }
simontruelove 11:74eeb8871fe6 282 }
simontruelove 1:0191658b6ff4 283 }
simontruelove 1:0191658b6ff4 284 }
simontruelove 1:0191658b6ff4 285 void StepCW(void) //Square wave switching
simontruelove 1:0191658b6ff4 286 {
simontruelove 1:0191658b6ff4 287 Ph1();
simontruelove 3:4249dbdf7ed3 288 wait(x);
simontruelove 13:da9d3fbbe407 289 //Ph12();
simontruelove 13:da9d3fbbe407 290 //wait(y);
simontruelove 1:0191658b6ff4 291 Ph2();
simontruelove 3:4249dbdf7ed3 292 wait(x);
simontruelove 13:da9d3fbbe407 293 //Ph23();
simontruelove 13:da9d3fbbe407 294 //wait(y);
simontruelove 1:0191658b6ff4 295 Ph3();
simontruelove 3:4249dbdf7ed3 296 wait(x);
simontruelove 13:da9d3fbbe407 297 //Ph34();
simontruelove 13:da9d3fbbe407 298 //wait(y);
simontruelove 1:0191658b6ff4 299 Ph4();
simontruelove 3:4249dbdf7ed3 300 wait(x);
simontruelove 13:da9d3fbbe407 301 //Ph41();
simontruelove 13:da9d3fbbe407 302 //wait(y);
simontruelove 13:da9d3fbbe407 303 }
simontruelove 13:da9d3fbbe407 304
simontruelove 13:da9d3fbbe407 305 void Ph0(void)
simontruelove 13:da9d3fbbe407 306 {
simontruelove 13:da9d3fbbe407 307 Phase1.write(0);
simontruelove 13:da9d3fbbe407 308 Phase2.write(0);
simontruelove 13:da9d3fbbe407 309 Phase3.write(0);
simontruelove 13:da9d3fbbe407 310 Phase4.write(0);
simontruelove 13:da9d3fbbe407 311 //wait(x);
simontruelove 13:da9d3fbbe407 312 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 1:0191658b6ff4 313 }
simontruelove 1:0191658b6ff4 314
simontruelove 1:0191658b6ff4 315 void Ph1(void)
simontruelove 1:0191658b6ff4 316 {
simontruelove 9:061600a6c750 317 Phase1.write(duty);
simontruelove 9:061600a6c750 318 Phase2.write(0);
simontruelove 9:061600a6c750 319 Phase3.write(0);
simontruelove 9:061600a6c750 320 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 321 //wait(x);
simontruelove 3:4249dbdf7ed3 322 //pc.printf("Phase 1 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 323 }
simontruelove 3:4249dbdf7ed3 324
simontruelove 3:4249dbdf7ed3 325 void Ph12 (void)
simontruelove 3:4249dbdf7ed3 326 {
simontruelove 9:061600a6c750 327 Phase1.write(duty);
simontruelove 9:061600a6c750 328 Phase2.write(duty);
simontruelove 9:061600a6c750 329 Phase3.write(0);
simontruelove 9:061600a6c750 330 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 331 //wait(y);
simontruelove 1:0191658b6ff4 332 }
simontruelove 1:0191658b6ff4 333
simontruelove 1:0191658b6ff4 334 void Ph2(void)
simontruelove 1:0191658b6ff4 335 {
simontruelove 9:061600a6c750 336 Phase1.write(0);
simontruelove 9:061600a6c750 337 Phase2.write(duty);
simontruelove 9:061600a6c750 338 Phase3.write(0);
simontruelove 9:061600a6c750 339 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 340 //wait(x);
simontruelove 3:4249dbdf7ed3 341 //pc.printf("Phase 2 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 342 }
simontruelove 3:4249dbdf7ed3 343
simontruelove 3:4249dbdf7ed3 344 void Ph23 (void)
simontruelove 3:4249dbdf7ed3 345 {
simontruelove 9:061600a6c750 346 Phase1.write(0);
simontruelove 9:061600a6c750 347 Phase2.write(duty);
simontruelove 9:061600a6c750 348 Phase3.write(duty);
simontruelove 9:061600a6c750 349 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 350 //wait(y);
simontruelove 1:0191658b6ff4 351 }
simontruelove 0:634dd505dace 352
simontruelove 1:0191658b6ff4 353 void Ph3(void)
simontruelove 1:0191658b6ff4 354 {
simontruelove 9:061600a6c750 355 Phase1.write(0);
simontruelove 9:061600a6c750 356 Phase2.write(0);
simontruelove 9:061600a6c750 357 Phase3.write(duty);
simontruelove 9:061600a6c750 358 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 359 //wait(x);
simontruelove 3:4249dbdf7ed3 360 //pc.printf("Phase 3 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 361 }
simontruelove 3:4249dbdf7ed3 362 void Ph34 (void)
simontruelove 3:4249dbdf7ed3 363 {
simontruelove 9:061600a6c750 364 Phase1.write(0);
simontruelove 9:061600a6c750 365 Phase2.write(0);
simontruelove 9:061600a6c750 366 Phase3.write(duty);
simontruelove 9:061600a6c750 367 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 368 //wait(y);
simontruelove 1:0191658b6ff4 369 }
simontruelove 1:0191658b6ff4 370
simontruelove 1:0191658b6ff4 371 void Ph4(void)
simontruelove 1:0191658b6ff4 372 {
simontruelove 9:061600a6c750 373 Phase1.write(0);
simontruelove 9:061600a6c750 374 Phase2.write(0);
simontruelove 9:061600a6c750 375 Phase3.write(0);
simontruelove 9:061600a6c750 376 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 377 //wait(x);
simontruelove 3:4249dbdf7ed3 378 //pc.printf("Phase 4 = %i\n\r", wheel.getPulses());
simontruelove 3:4249dbdf7ed3 379 }
simontruelove 3:4249dbdf7ed3 380
simontruelove 3:4249dbdf7ed3 381 void Ph41 (void)
simontruelove 3:4249dbdf7ed3 382 {
simontruelove 9:061600a6c750 383 Phase1.write(duty);
simontruelove 9:061600a6c750 384 Phase2.write(0);
simontruelove 9:061600a6c750 385 Phase3.write(0);
simontruelove 9:061600a6c750 386 Phase4.write(duty);
simontruelove 3:4249dbdf7ed3 387 //wait(y);
simontruelove 3:4249dbdf7ed3 388 }
simontruelove 3:4249dbdf7ed3 389
simontruelove 10:808cb9052f14 390 void Initialisation (void) //Turn everything off
simontruelove 3:4249dbdf7ed3 391 {
simontruelove 9:061600a6c750 392 Phase1.write(0);
simontruelove 9:061600a6c750 393 Phase2.write(0);
simontruelove 9:061600a6c750 394 Phase3.write(0);
simontruelove 9:061600a6c750 395 Phase4.write(0);
simontruelove 3:4249dbdf7ed3 396 led1 = 0;
simontruelove 3:4249dbdf7ed3 397 led2 = 0;
simontruelove 3:4249dbdf7ed3 398 led3 = 0;
simontruelove 3:4249dbdf7ed3 399 led4 = 0;
simontruelove 12:cbea987a3ec4 400 UnUsedPhase1=0;
simontruelove 12:cbea987a3ec4 401 UnUsedPhase2=0;
simontruelove 3:4249dbdf7ed3 402 wheel.ResetYay();
simontruelove 4:3aedc9246ae4 403 }
simontruelove 4:3aedc9246ae4 404
simontruelove 10:808cb9052f14 405 void GetChar (void) //read keyboard strikes with terraterm
simontruelove 4:3aedc9246ae4 406 { if (pc.readable())
simontruelove 4:3aedc9246ae4 407 {
simontruelove 4:3aedc9246ae4 408 c = pc.getc();
simontruelove 10:808cb9052f14 409 if(c == 'z') //turn on led1 causes CW operation
simontruelove 4:3aedc9246ae4 410 {
simontruelove 13:da9d3fbbe407 411 AdjCW = 0;
simontruelove 13:da9d3fbbe407 412 AdjACW = 10;
simontruelove 4:3aedc9246ae4 413 led1 = !led1;
simontruelove 4:3aedc9246ae4 414 led2 = 0;
simontruelove 13:da9d3fbbe407 415 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 416 }
simontruelove 10:808cb9052f14 417 if(c == 'x') //turn on led2 causes ACW operation
simontruelove 4:3aedc9246ae4 418 {
simontruelove 13:da9d3fbbe407 419 AdjCW = 0;
simontruelove 13:da9d3fbbe407 420 AdjACW = 10;
simontruelove 4:3aedc9246ae4 421 led1 = 0;
simontruelove 4:3aedc9246ae4 422 led2 = !led2 ;
simontruelove 10:808cb9052f14 423 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 4:3aedc9246ae4 424 }
simontruelove 10:808cb9052f14 425 if(c == 'q') //Increases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 426 {
simontruelove 9:061600a6c750 427 //duty = duty + 0.0001;
simontruelove 13:da9d3fbbe407 428 SetPoint=SetPoint+5;
simontruelove 10:808cb9052f14 429 if (SetPoint >2200)
simontruelove 10:808cb9052f14 430 {
simontruelove 10:808cb9052f14 431 SetPoint = 2200;
simontruelove 10:808cb9052f14 432 }
simontruelove 10:808cb9052f14 433 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 7:b8de1529c7fc 434 }
simontruelove 10:808cb9052f14 435 if(c == 'a') //Decreases setpoint used in Velocity loop
simontruelove 7:b8de1529c7fc 436 {
simontruelove 9:061600a6c750 437 //duty = duty - 0.0001;
simontruelove 13:da9d3fbbe407 438 SetPoint=SetPoint-5;
simontruelove 10:808cb9052f14 439 if (SetPoint <50)
simontruelove 9:061600a6c750 440 {
simontruelove 10:808cb9052f14 441 SetPoint = 50;
simontruelove 9:061600a6c750 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== 'o')
simontruelove 10:808cb9052f14 446 {
simontruelove 10:808cb9052f14 447 AdjCW = AdjCW+1;
simontruelove 10:808cb9052f14 448 if (AdjCW >15)
simontruelove 10:808cb9052f14 449 {
simontruelove 10:808cb9052f14 450 AdjCW = 0;
simontruelove 10:808cb9052f14 451 }
simontruelove 10:808cb9052f14 452 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 453 }
simontruelove 10:808cb9052f14 454 if(c== 'k')
simontruelove 10:808cb9052f14 455 {
simontruelove 10:808cb9052f14 456 AdjCW = AdjCW-1;
simontruelove 10:808cb9052f14 457 if (AdjCW <0)
simontruelove 10:808cb9052f14 458 {
simontruelove 10:808cb9052f14 459 AdjCW = 15;
simontruelove 10:808cb9052f14 460 }
simontruelove 10:808cb9052f14 461 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 462 }
simontruelove 10:808cb9052f14 463 if(c== 'p')
simontruelove 10:808cb9052f14 464 {
simontruelove 10:808cb9052f14 465 AdjACW = AdjACW+1;
simontruelove 10:808cb9052f14 466 if (AdjACW >15)
simontruelove 10:808cb9052f14 467 {
simontruelove 10:808cb9052f14 468 AdjACW = 0;
simontruelove 10:808cb9052f14 469 }
simontruelove 10:808cb9052f14 470 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 471 }
simontruelove 10:808cb9052f14 472 if(c== 'l')
simontruelove 10:808cb9052f14 473 {
simontruelove 10:808cb9052f14 474 AdjACW = AdjACW-1;
simontruelove 10:808cb9052f14 475 if (AdjACW <0)
simontruelove 10:808cb9052f14 476 {
simontruelove 10:808cb9052f14 477 AdjACW = 15;
simontruelove 10:808cb9052f14 478 }
simontruelove 10:808cb9052f14 479 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 10:808cb9052f14 480 }
simontruelove 10:808cb9052f14 481 if(c=='0')
simontruelove 10:808cb9052f14 482 {
simontruelove 10:808cb9052f14 483 pc.printf("%i, %.5f, %i, %i, %i \n\r", SetPoint, duty, AdjCW, AdjACW, rpm);
simontruelove 8:2e690f407ec6 484 }
simontruelove 11:74eeb8871fe6 485 if(c=='t')
simontruelove 11:74eeb8871fe6 486 {
simontruelove 12:cbea987a3ec4 487 pc.printf("%.0f C\n\r",temp);
simontruelove 11:74eeb8871fe6 488 }
simontruelove 4:3aedc9246ae4 489 }
simontruelove 5:4e5c644d5cc3 490 }
simontruelove 5:4e5c644d5cc3 491
simontruelove 5:4e5c644d5cc3 492 void RPM (void)
simontruelove 5:4e5c644d5cc3 493 {
simontruelove 12:cbea987a3ec4 494 wheel.ResetWhoop(); //PulseCount2_==800, whoop_=1;
simontruelove 10:808cb9052f14 495 TimePerClick = (t.read_us()); //read timer in microseconds
simontruelove 10:808cb9052f14 496 t.reset(); //reset timer
simontruelove 12:cbea987a3ec4 497 TimePerRev = TimePerClick * (800/z); //z = 800 (PulseCount2_==800)
simontruelove 10:808cb9052f14 498 TimePerRev = TimePerRev / 1000; //
simontruelove 5:4e5c644d5cc3 499 RPS = 10000000 / TimePerRev;
simontruelove 5:4e5c644d5cc3 500 rpm = (RPS * 60)/10000;
simontruelove 12:cbea987a3ec4 501 //Aout=((0.30303*rpm)/1000); // for 500 rpm (0.30303*500/1000)*3.3V = 0.500V
simontruelove 11:74eeb8871fe6 502 ReadKType();
simontruelove 13:da9d3fbbe407 503 if(rpm < 10)
simontruelove 13:da9d3fbbe407 504 {
simontruelove 13:da9d3fbbe407 505 AdjCW = 0;
simontruelove 13:da9d3fbbe407 506 AdjACW = 10;
simontruelove 13:da9d3fbbe407 507 }
simontruelove 13:da9d3fbbe407 508 if(rpm > 9 and rpm < 124)
simontruelove 13:da9d3fbbe407 509 {
simontruelove 13:da9d3fbbe407 510 AdjCW = 3;
simontruelove 13:da9d3fbbe407 511 AdjACW = 7;
simontruelove 13:da9d3fbbe407 512 }
simontruelove 13:da9d3fbbe407 513 if(rpm > 125 and rpm < 319)
simontruelove 13:da9d3fbbe407 514 {
simontruelove 13:da9d3fbbe407 515 AdjCW = 4;
simontruelove 13:da9d3fbbe407 516 AdjACW = 6;
simontruelove 13:da9d3fbbe407 517 }
simontruelove 13:da9d3fbbe407 518 if(rpm > 320 and rpm < 399)
simontruelove 13:da9d3fbbe407 519 {
simontruelove 13:da9d3fbbe407 520 AdjCW = 5;
simontruelove 13:da9d3fbbe407 521 AdjACW = 6;
simontruelove 13:da9d3fbbe407 522 }
simontruelove 13:da9d3fbbe407 523 if(rpm > 400)
simontruelove 13:da9d3fbbe407 524 {
simontruelove 13:da9d3fbbe407 525 AdjCW = 5;
simontruelove 13:da9d3fbbe407 526 AdjACW = 5;
simontruelove 13:da9d3fbbe407 527 }
simontruelove 13:da9d3fbbe407 528
simontruelove 10:808cb9052f14 529 //pc.printf("rpm = %i\r", rpm);
simontruelove 6:f7028034aabb 530 //pc.printf("StateA= %i, StateB= %i, StateC= %i, Pulses = %i\n\r", StateA, StateB, StateC, wheel.getPulses());
simontruelove 7:b8de1529c7fc 531 }
simontruelove 7:b8de1529c7fc 532
simontruelove 7:b8de1529c7fc 533 void VelocityLoop (void)
simontruelove 7:b8de1529c7fc 534 {
simontruelove 10:808cb9052f14 535 diff = SetPoint - rpm; //difference between setpoint and the RPM measurement
simontruelove 13:da9d3fbbe407 536 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 10:808cb9052f14 537 if (duty > 1) //limits for duty. Motor will not operate below 0.96. 1 = max
simontruelove 7:b8de1529c7fc 538 {
simontruelove 9:061600a6c750 539 duty = 1;
simontruelove 7:b8de1529c7fc 540 }
simontruelove 13:da9d3fbbe407 541 if (duty <0.32490) //3A min duty 0.96, 6.5A min duty 0.4
simontruelove 10:808cb9052f14 542 {
simontruelove 13:da9d3fbbe407 543 duty = 0.32490;
simontruelove 7:b8de1529c7fc 544 }
simontruelove 10:808cb9052f14 545 //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 546 }
simontruelove 11:74eeb8871fe6 547
simontruelove 11:74eeb8871fe6 548 void ReadKType(void)
simontruelove 11:74eeb8871fe6 549 {
simontruelove 11:74eeb8871fe6 550 int i = 0;
simontruelove 11:74eeb8871fe6 551 int Readout = 0;
simontruelove 11:74eeb8871fe6 552
simontruelove 11:74eeb8871fe6 553 cs1 = 0;
simontruelove 11:74eeb8871fe6 554 //SerialClock = 0; //set clock to 0
simontruelove 11:74eeb8871fe6 555 //wait_ms(1);
simontruelove 11:74eeb8871fe6 556 SerialClock = 1; //clock once to set to the 13 bit temp data
simontruelove 12:cbea987a3ec4 557 wait_ms(0.1);
simontruelove 11:74eeb8871fe6 558 SerialClock = 0;
simontruelove 12:cbea987a3ec4 559 wait_ms(0.1);
simontruelove 11:74eeb8871fe6 560
simontruelove 11:74eeb8871fe6 561 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 562 {
simontruelove 11:74eeb8871fe6 563 if(DOut == 1) //check data, store results in readout
simontruelove 11:74eeb8871fe6 564 {
simontruelove 12:cbea987a3ec4 565 Readout |= (1<<i);
simontruelove 11:74eeb8871fe6 566 }
simontruelove 11:74eeb8871fe6 567 else
simontruelove 11:74eeb8871fe6 568 {
simontruelove 11:74eeb8871fe6 569 Readout |= (0<<i);
simontruelove 11:74eeb8871fe6 570 }
simontruelove 12:cbea987a3ec4 571
simontruelove 11:74eeb8871fe6 572 SerialClock = 1; //clock to the next bit
simontruelove 12:cbea987a3ec4 573 wait_ms(0.1);
simontruelove 11:74eeb8871fe6 574 SerialClock = 0;
simontruelove 12:cbea987a3ec4 575 wait_ms(0.1);
simontruelove 11:74eeb8871fe6 576 }
simontruelove 11:74eeb8871fe6 577 temp = Readout * 0.125; //get the real temp value which is a float
simontruelove 11:74eeb8871fe6 578 //pc.printf("%f\n\r",temp);
simontruelove 11:74eeb8871fe6 579 Readout = 0;
simontruelove 11:74eeb8871fe6 580 cs1 = 1;
simontruelove 3:4249dbdf7ed3 581 }