Stewart Coulden-Smith / EquatorStrutController

Equator Strut Controller

Dependents:   EquatorStrutDigitalMonitor

EquatorStrutController.cpp@2:3976e3f43470, 2014-08-20 (annotated)

Committer:
pyrostew
Date:
Wed Aug 20 08:34:59 2014 +0000
Revision:
2:3976e3f43470
Parent:
1:580fded7b5b2 
Updated library to latest code used in Alpeshs' project.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pyrostew 0:a6ade00ff41d 1 #include "EquatorStrutController.h"
pyrostew 0:a6ade00ff41d 2
pyrostew 0:a6ade00ff41d 3 EquatorStrut::EquatorStrut()
pyrostew 0:a6ade00ff41d 4 {
pyrostew 2:3976e3f43470 5 position = 0;
pyrostew 0:a6ade00ff41d 6 direction = 0;
pyrostew 0:a6ade00ff41d 7 Homing = false;
pyrostew 0:a6ade00ff41d 8 HallTriggered = false;
pyrostew 0:a6ade00ff41d 9 Enabled = true;
pyrostew 0:a6ade00ff41d 10
pyrostew 0:a6ade00ff41d 11 ResetLine = new DigitalOut(P1_29);
pyrostew 0:a6ade00ff41d 12
pyrostew 0:a6ade00ff41d 13 Disable();
pyrostew 0:a6ade00ff41d 14
pyrostew 2:3976e3f43470 15 RGHSinInterrupt = new InterruptIn(P0_11);
pyrostew 2:3976e3f43470 16 RGHCosInterrupt = new InterruptIn(P0_12);
pyrostew 2:3976e3f43470 17 RGHSinFallingInterrupt = new InterruptIn(P0_13);
pyrostew 2:3976e3f43470 18 RGHCosFallingInterrupt = new InterruptIn(P0_14);
pyrostew 2:3976e3f43470 19
pyrostew 0:a6ade00ff41d 20 HallSensor = new InterruptIn(P0_2);
pyrostew 2:3976e3f43470 21 HallSensorState = new DigitalIn(P0_2);
pyrostew 0:a6ade00ff41d 22
pyrostew 2:3976e3f43470 23 (*RunningTime).start();
pyrostew 2:3976e3f43470 24
pyrostew 2:3976e3f43470 25 (*RGHSinInterrupt).rise(this, &EquatorStrut::RGHSinRisingHandler);
pyrostew 2:3976e3f43470 26 (*RGHCosInterrupt).rise(this, &EquatorStrut::RGHCosRisingHandler);
pyrostew 2:3976e3f43470 27 (*RGHSinFallingInterrupt).fall(this, &EquatorStrut::RGHSinFallingHandler);
pyrostew 2:3976e3f43470 28 (*RGHCosFallingInterrupt).fall(this, &EquatorStrut::RGHCosFallingHandler);
pyrostew 2:3976e3f43470 29
pyrostew 0:a6ade00ff41d 30 (*HallSensor).fall(this, &EquatorStrut::HallEffectFall);
pyrostew 0:a6ade00ff41d 31 (*HallSensor).mode(PullUp);
pyrostew 0:a6ade00ff41d 32
pyrostew 0:a6ade00ff41d 33 PhaseA = new PwmOut(P0_9);
pyrostew 0:a6ade00ff41d 34 PhaseB = new PwmOut(P0_8);
pyrostew 2:3976e3f43470 35 }
pyrostew 2:3976e3f43470 36
pyrostew 2:3976e3f43470 37 bool EquatorStrut::IsEnabled()
pyrostew 2:3976e3f43470 38 {
pyrostew 2:3976e3f43470 39 return Enabled;
pyrostew 2:3976e3f43470 40 }
pyrostew 2:3976e3f43470 41
pyrostew 2:3976e3f43470 42 void EquatorStrut::ActionEvent(bool CurrHigh, bool CurrSin)
pyrostew 2:3976e3f43470 43 {
pyrostew 2:3976e3f43470 44 // Same event again - DO NOTHING
pyrostew 2:3976e3f43470 45 if (CurrHigh == LastHigh && CurrSin == LastSin)
pyrostew 2:3976e3f43470 46 {
pyrostew 2:3976e3f43470 47 return;
pyrostew 2:3976e3f43470 48 }
pyrostew 0:a6ade00ff41d 49
pyrostew 2:3976e3f43470 50 if (CurrSin != LastSin) // Otherwave
pyrostew 2:3976e3f43470 51 {
pyrostew 2:3976e3f43470 52 // Other wave
pyrostew 2:3976e3f43470 53 if ((CurrSin && CurrHigh == LastHigh) ||
pyrostew 2:3976e3f43470 54 (!CurrSin && CurrHigh != LastHigh))
pyrostew 2:3976e3f43470 55 {
pyrostew 2:3976e3f43470 56 //Forwards
pyrostew 2:3976e3f43470 57 direction = 1;
pyrostew 2:3976e3f43470 58 }
pyrostew 2:3976e3f43470 59 else
pyrostew 2:3976e3f43470 60 {
pyrostew 2:3976e3f43470 61 //Backwards
pyrostew 2:3976e3f43470 62 direction = -1;
pyrostew 2:3976e3f43470 63 }
pyrostew 2:3976e3f43470 64
pyrostew 2:3976e3f43470 65
pyrostew 2:3976e3f43470 66 }
pyrostew 2:3976e3f43470 67 else
pyrostew 2:3976e3f43470 68 {
pyrostew 2:3976e3f43470 69 // Reversal
pyrostew 2:3976e3f43470 70 direction = -direction;
pyrostew 2:3976e3f43470 71 }
pyrostew 2:3976e3f43470 72
pyrostew 2:3976e3f43470 73 position += direction;
pyrostew 2:3976e3f43470 74
pyrostew 2:3976e3f43470 75 // Set the state for the wave that fired
pyrostew 2:3976e3f43470 76 if(CurrSin) SinHigh = CurrHigh;
pyrostew 2:3976e3f43470 77 else CosHigh = CurrHigh;
pyrostew 2:3976e3f43470 78
pyrostew 2:3976e3f43470 79 // Set the last event values
pyrostew 2:3976e3f43470 80 LastHigh = CurrHigh;
pyrostew 2:3976e3f43470 81 LastSin = CurrSin;
pyrostew 2:3976e3f43470 82 }
pyrostew 2:3976e3f43470 83
pyrostew 2:3976e3f43470 84 void EquatorStrut::DisableInterrupts()
pyrostew 2:3976e3f43470 85 {
pyrostew 2:3976e3f43470 86 (*RGHSinInterrupt).disable_irq();
pyrostew 2:3976e3f43470 87 (*RGHCosInterrupt).disable_irq();
pyrostew 2:3976e3f43470 88 (*RGHSinFallingInterrupt).disable_irq();
pyrostew 2:3976e3f43470 89 (*RGHCosFallingInterrupt).disable_irq();
pyrostew 2:3976e3f43470 90 }
pyrostew 2:3976e3f43470 91
pyrostew 2:3976e3f43470 92 void EquatorStrut::EnableInterrupts()
pyrostew 2:3976e3f43470 93 {
pyrostew 2:3976e3f43470 94 (*RGHSinInterrupt).enable_irq();
pyrostew 2:3976e3f43470 95 (*RGHCosInterrupt).enable_irq();
pyrostew 2:3976e3f43470 96 (*RGHSinFallingInterrupt).enable_irq();
pyrostew 2:3976e3f43470 97 (*RGHCosFallingInterrupt).enable_irq();
pyrostew 0:a6ade00ff41d 98 }
pyrostew 0:a6ade00ff41d 99
pyrostew 0:a6ade00ff41d 100 void EquatorStrut::SetPower(double power)
pyrostew 0:a6ade00ff41d 101 {
pyrostew 0:a6ade00ff41d 102 if(!Enabled)
pyrostew 0:a6ade00ff41d 103 {
pyrostew 0:a6ade00ff41d 104 return;
pyrostew 0:a6ade00ff41d 105 }
pyrostew 0:a6ade00ff41d 106
pyrostew 2:3976e3f43470 107 if (fabs(power) > 1.0) return;
pyrostew 0:a6ade00ff41d 108
pyrostew 2:3976e3f43470 109 currentPower = power;
pyrostew 2:3976e3f43470 110
pyrostew 2:3976e3f43470 111 (*PhaseA) = (power + 1.0) / 2;
pyrostew 2:3976e3f43470 112 (*PhaseB) = 1.0 - ((power + 1.0) / 2);
pyrostew 0:a6ade00ff41d 113 }
pyrostew 0:a6ade00ff41d 114
pyrostew 0:a6ade00ff41d 115 double EquatorStrut::GetPosition()
pyrostew 0:a6ade00ff41d 116 {
pyrostew 2:3976e3f43470 117 return position * 0.01;
pyrostew 0:a6ade00ff41d 118 }
pyrostew 0:a6ade00ff41d 119
pyrostew 0:a6ade00ff41d 120 void EquatorStrut::Home()
pyrostew 0:a6ade00ff41d 121 {
pyrostew 0:a6ade00ff41d 122 if (!Enabled)
pyrostew 0:a6ade00ff41d 123 {
pyrostew 0:a6ade00ff41d 124 Enable();
pyrostew 0:a6ade00ff41d 125 }
pyrostew 0:a6ade00ff41d 126
pyrostew 2:3976e3f43470 127 if ((*HallSensorState) == 1)
pyrostew 0:a6ade00ff41d 128 {
pyrostew 2:3976e3f43470 129 DisableInterrupts();
pyrostew 2:3976e3f43470 130
pyrostew 2:3976e3f43470 131 direction = -1;
pyrostew 2:3976e3f43470 132
pyrostew 2:3976e3f43470 133 Homing = true;
pyrostew 2:3976e3f43470 134 HallTriggered = false;
pyrostew 2:3976e3f43470 135
pyrostew 2:3976e3f43470 136 SetPower(-0.6);
pyrostew 2:3976e3f43470 137
pyrostew 2:3976e3f43470 138 while (!HallTriggered)
pyrostew 2:3976e3f43470 139 {
pyrostew 2:3976e3f43470 140 wait(0.1);
pyrostew 2:3976e3f43470 141 }
pyrostew 2:3976e3f43470 142
pyrostew 2:3976e3f43470 143 EnableInterrupts();
pyrostew 0:a6ade00ff41d 144 }
pyrostew 0:a6ade00ff41d 145
pyrostew 0:a6ade00ff41d 146 SetPower(1.0);
pyrostew 2:3976e3f43470 147
pyrostew 2:3976e3f43470 148 while (position < 2000)
pyrostew 0:a6ade00ff41d 149 {
pyrostew 2:3976e3f43470 150 //SerialTransmit();
pyrostew 0:a6ade00ff41d 151 }
pyrostew 0:a6ade00ff41d 152
pyrostew 0:a6ade00ff41d 153 Homing = true;
pyrostew 2:3976e3f43470 154 HallTriggered = false;
pyrostew 0:a6ade00ff41d 155
pyrostew 2:3976e3f43470 156 DisableInterrupts();
pyrostew 2:3976e3f43470 157
pyrostew 2:3976e3f43470 158 direction = -1;
pyrostew 2:3976e3f43470 159
pyrostew 2:3976e3f43470 160 SetPower(-0.4);
pyrostew 0:a6ade00ff41d 161
pyrostew 0:a6ade00ff41d 162 while (!HallTriggered)
pyrostew 0:a6ade00ff41d 163 {
pyrostew 2:3976e3f43470 164 wait(0.1);
pyrostew 0:a6ade00ff41d 165 }
pyrostew 2:3976e3f43470 166
pyrostew 2:3976e3f43470 167 EnableInterrupts();
pyrostew 0:a6ade00ff41d 168 }
pyrostew 0:a6ade00ff41d 169
pyrostew 0:a6ade00ff41d 170 void EquatorStrut::Enable()
pyrostew 0:a6ade00ff41d 171 {
pyrostew 0:a6ade00ff41d 172 SetPower(0.0);
pyrostew 0:a6ade00ff41d 173
pyrostew 0:a6ade00ff41d 174 (*ResetLine) = 1;
pyrostew 0:a6ade00ff41d 175
pyrostew 0:a6ade00ff41d 176 Enabled = true;
pyrostew 0:a6ade00ff41d 177 }
pyrostew 0:a6ade00ff41d 178
pyrostew 0:a6ade00ff41d 179 void EquatorStrut::Disable()
pyrostew 0:a6ade00ff41d 180 {
pyrostew 0:a6ade00ff41d 181 (*ResetLine) = 0;
pyrostew 0:a6ade00ff41d 182
pyrostew 0:a6ade00ff41d 183 SetPower(0.0);
pyrostew 0:a6ade00ff41d 184
pyrostew 0:a6ade00ff41d 185 Enabled = false;
pyrostew 0:a6ade00ff41d 186 }
pyrostew 0:a6ade00ff41d 187
pyrostew 0:a6ade00ff41d 188 double EquatorStrut::CurrentSpeed()
pyrostew 0:a6ade00ff41d 189 {
pyrostew 2:3976e3f43470 190 double interval = (*RunningTime).read() - SpeedInterval;
pyrostew 2:3976e3f43470 191 int positionDiff = position - LastPosition;
pyrostew 2:3976e3f43470 192
pyrostew 2:3976e3f43470 193 SpeedInterval = (*RunningTime).read();
pyrostew 2:3976e3f43470 194 LastPosition = position;
pyrostew 2:3976e3f43470 195
pyrostew 2:3976e3f43470 196 return (positionDiff * 0.01)/interval;
pyrostew 2:3976e3f43470 197 }
pyrostew 2:3976e3f43470 198
pyrostew 2:3976e3f43470 199 double EquatorStrut::CurrentPower()
pyrostew 2:3976e3f43470 200 {
pyrostew 2:3976e3f43470 201 return currentPower;
pyrostew 0:a6ade00ff41d 202 }
pyrostew 0:a6ade00ff41d 203
pyrostew 2:3976e3f43470 204 void EquatorStrut::RGHSinRisingHandler()
pyrostew 2:3976e3f43470 205 {
pyrostew 2:3976e3f43470 206 ActionEvent(true, true);
pyrostew 2:3976e3f43470 207 }
pyrostew 2:3976e3f43470 208
pyrostew 2:3976e3f43470 209 void EquatorStrut::RGHSinFallingHandler()
pyrostew 2:3976e3f43470 210 {
pyrostew 2:3976e3f43470 211 ActionEvent(false, true);
pyrostew 2:3976e3f43470 212 }
pyrostew 2:3976e3f43470 213
pyrostew 2:3976e3f43470 214 void EquatorStrut::RGHCosRisingHandler()
pyrostew 2:3976e3f43470 215 {
pyrostew 2:3976e3f43470 216 ActionEvent(true, false);
pyrostew 2:3976e3f43470 217 }
pyrostew 2:3976e3f43470 218
pyrostew 2:3976e3f43470 219 void EquatorStrut::RGHCosFallingHandler()
pyrostew 2:3976e3f43470 220 {
pyrostew 2:3976e3f43470 221 ActionEvent(false, false);
pyrostew 0:a6ade00ff41d 222 }
pyrostew 0:a6ade00ff41d 223
pyrostew 0:a6ade00ff41d 224 void EquatorStrut::HallEffectFall()
pyrostew 0:a6ade00ff41d 225 {
pyrostew 0:a6ade00ff41d 226 if (direction < 0)
pyrostew 0:a6ade00ff41d 227 {
pyrostew 0:a6ade00ff41d 228 SetPower(0.0);
pyrostew 0:a6ade00ff41d 229
pyrostew 0:a6ade00ff41d 230 if (Homing)
pyrostew 0:a6ade00ff41d 231 {
pyrostew 0:a6ade00ff41d 232 HallTriggered = true;
pyrostew 0:a6ade00ff41d 233 Homing = false;
pyrostew 0:a6ade00ff41d 234 position = 0.0;
pyrostew 0:a6ade00ff41d 235 }
pyrostew 0:a6ade00ff41d 236 }
pyrostew 0:a6ade00ff41d 237 }