Class used to interface with the handheld gamepad.

Fork of Gamepad by Craig Evans

Committer:
eencae
Date:
Mon Feb 06 19:40:40 2017 +0000
Revision:
4:bafb7f483e93
Parent:
3:964a6d95acdd
Child:
7:019671f7bd83
Removed references to LCD and Joystick libraries.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eencae 0:a6288c29b936 1 #include "Gamepad.h"
eencae 0:a6288c29b936 2
eencae 1:6d25cd49059b 3 //////////// constructor/destructor ////////////
eencae 0:a6288c29b936 4 Gamepad::Gamepad()
eencae 0:a6288c29b936 5 {
eencae 0:a6288c29b936 6 led_1 = new PwmOut(PTA1);
eencae 0:a6288c29b936 7 led_2 = new PwmOut(PTA2);
eencae 0:a6288c29b936 8 led_3 = new PwmOut(PTC2);
eencae 0:a6288c29b936 9 led_4 = new PwmOut(PTC3);
eencae 0:a6288c29b936 10 led_5 = new PwmOut(PTC4);
eencae 0:a6288c29b936 11 led_6 = new PwmOut(PTD3);
eencae 0:a6288c29b936 12
eencae 0:a6288c29b936 13 button_A = new InterruptIn(PTB9);
eencae 2:ea5538fcfe2f 14 button_B = new InterruptIn(PTD0);
eencae 0:a6288c29b936 15 button_X = new InterruptIn(PTC17);
eencae 2:ea5538fcfe2f 16 button_Y = new InterruptIn(PTC12);
eencae 0:a6288c29b936 17 button_back = new InterruptIn(PTB19);
eencae 0:a6288c29b936 18 button_start = new InterruptIn(PTC5);
eencae 0:a6288c29b936 19 button_L = new InterruptIn(PTB18);
eencae 0:a6288c29b936 20 button_R = new InterruptIn(PTB3);
eencae 3:964a6d95acdd 21 button_joystick = new InterruptIn(PTC16);
eencae 3:964a6d95acdd 22
eencae 3:964a6d95acdd 23 vert = new AnalogIn(PTB10);
eencae 3:964a6d95acdd 24 horiz = new AnalogIn(PTB11);
eencae 0:a6288c29b936 25
eencae 0:a6288c29b936 26 buzzer = new PwmOut(PTC10);
eencae 0:a6288c29b936 27 pot = new AnalogIn(PTB2);
eencae 0:a6288c29b936 28
eencae 1:6d25cd49059b 29 timeout = new Timeout();
eencae 1:6d25cd49059b 30
eencae 0:a6288c29b936 31 }
eencae 0:a6288c29b936 32
eencae 1:6d25cd49059b 33 Gamepad::~Gamepad()
eencae 1:6d25cd49059b 34 {
eencae 1:6d25cd49059b 35 delete led_1,led_2,led_3,led_4,led_5,led_6;
eencae 3:964a6d95acdd 36 delete button_A,button_B,button_joystick,vert,horiz;
eencae 1:6d25cd49059b 37 delete button_X, button_Y, button_back, button_start;
eencae 1:6d25cd49059b 38 delete button_L, button_R, buzzer, pot, timeout;
eencae 1:6d25cd49059b 39 }
eencae 1:6d25cd49059b 40
eencae 1:6d25cd49059b 41 ///////////////// public methods /////////////////
eencae 1:6d25cd49059b 42
eencae 0:a6288c29b936 43 void Gamepad::init()
eencae 0:a6288c29b936 44 {
eencae 1:6d25cd49059b 45 leds_off();
eencae 3:964a6d95acdd 46
eencae 3:964a6d95acdd 47 // read centred values of joystick
eencae 3:964a6d95acdd 48 _x0 = horiz->read();
eencae 3:964a6d95acdd 49 _y0 = vert->read();
eencae 1:6d25cd49059b 50
eencae 1:6d25cd49059b 51 buzzer->period(1.0/1000.0); // 1 kHz
eencae 1:6d25cd49059b 52
eencae 1:6d25cd49059b 53 // clear all flags
eencae 3:964a6d95acdd 54 a_flag=0,b_flag=0,x_flag=0,y_flag=0,joy_flag=0;
eencae 3:964a6d95acdd 55 l_flag=0,r_flag=0,back_flag=0,start_flag=0;
eencae 1:6d25cd49059b 56
eencae 0:a6288c29b936 57 }
eencae 0:a6288c29b936 58
eencae 0:a6288c29b936 59 void Gamepad::leds_off()
eencae 0:a6288c29b936 60 {
eencae 1:6d25cd49059b 61 fade_leds(0.0);
eencae 0:a6288c29b936 62 }
eencae 0:a6288c29b936 63
eencae 0:a6288c29b936 64 void Gamepad::leds_on()
eencae 0:a6288c29b936 65 {
eencae 1:6d25cd49059b 66 fade_leds(1.0);
eencae 1:6d25cd49059b 67 }
eencae 1:6d25cd49059b 68
eencae 1:6d25cd49059b 69 void Gamepad::fade_leds(float val)
eencae 1:6d25cd49059b 70 {
eencae 1:6d25cd49059b 71 if (val < 0.0f) {
eencae 1:6d25cd49059b 72 val = 0.0f;
eencae 1:6d25cd49059b 73 }
eencae 1:6d25cd49059b 74 if (val > 1.0f) {
eencae 1:6d25cd49059b 75 val = 1.0f;
eencae 1:6d25cd49059b 76 }
eencae 1:6d25cd49059b 77
eencae 1:6d25cd49059b 78 // leds are active-low, so subtract from 1.0
eencae 1:6d25cd49059b 79 // 0.0 corresponds to fully-off, 1.0 to fully-on
eencae 1:6d25cd49059b 80 val = 1.0f - val;
eencae 1:6d25cd49059b 81
eencae 1:6d25cd49059b 82 led_1->write(val);
eencae 1:6d25cd49059b 83 led_2->write(val);
eencae 1:6d25cd49059b 84 led_3->write(val);
eencae 1:6d25cd49059b 85 led_4->write(val);
eencae 1:6d25cd49059b 86 led_5->write(val);
eencae 1:6d25cd49059b 87 led_6->write(val);
eencae 1:6d25cd49059b 88 }
eencae 1:6d25cd49059b 89
eencae 1:6d25cd49059b 90 float Gamepad::read_pot()
eencae 1:6d25cd49059b 91 {
eencae 1:6d25cd49059b 92 return pot->read();
eencae 1:6d25cd49059b 93 }
eencae 1:6d25cd49059b 94
eencae 1:6d25cd49059b 95 void Gamepad::tone(float frequency, float duration)
eencae 1:6d25cd49059b 96 {
eencae 1:6d25cd49059b 97 buzzer->period(1.0f/frequency);
eencae 1:6d25cd49059b 98 buzzer->write(0.5); // 50% duty cycle - square wave
eencae 1:6d25cd49059b 99 timeout->attach(callback(this, &Gamepad::tone_off), duration );
eencae 1:6d25cd49059b 100 }
eencae 1:6d25cd49059b 101
eencae 1:6d25cd49059b 102 bool Gamepad::a_pressed()
eencae 1:6d25cd49059b 103 {
eencae 1:6d25cd49059b 104 // ISR must have been triggered
eencae 1:6d25cd49059b 105 if (a_flag) {
eencae 1:6d25cd49059b 106 a_flag = 0; // clear flag
eencae 1:6d25cd49059b 107 return true;
eencae 1:6d25cd49059b 108 } else {
eencae 1:6d25cd49059b 109 return false;
eencae 1:6d25cd49059b 110 }
eencae 1:6d25cd49059b 111 }
eencae 1:6d25cd49059b 112
eencae 1:6d25cd49059b 113 bool Gamepad::b_pressed()
eencae 1:6d25cd49059b 114 {
eencae 1:6d25cd49059b 115 // ISR must have been triggered
eencae 1:6d25cd49059b 116 if (b_flag) {
eencae 1:6d25cd49059b 117 b_flag = 0; // clear flag
eencae 1:6d25cd49059b 118 return true;
eencae 1:6d25cd49059b 119 } else {
eencae 1:6d25cd49059b 120 return false;
eencae 1:6d25cd49059b 121 }
eencae 1:6d25cd49059b 122 }
eencae 1:6d25cd49059b 123
eencae 1:6d25cd49059b 124 bool Gamepad::x_pressed()
eencae 1:6d25cd49059b 125 {
eencae 1:6d25cd49059b 126 // ISR must have been triggered
eencae 1:6d25cd49059b 127 if (x_flag) {
eencae 1:6d25cd49059b 128 x_flag = 0; // clear flag
eencae 1:6d25cd49059b 129 return true;
eencae 1:6d25cd49059b 130 } else {
eencae 1:6d25cd49059b 131 return false;
eencae 1:6d25cd49059b 132 }
eencae 1:6d25cd49059b 133 }
eencae 1:6d25cd49059b 134
eencae 1:6d25cd49059b 135 bool Gamepad::y_pressed()
eencae 1:6d25cd49059b 136 {
eencae 1:6d25cd49059b 137 // ISR must have been triggered
eencae 1:6d25cd49059b 138 if (y_flag) {
eencae 1:6d25cd49059b 139 y_flag = 0; // clear flag
eencae 1:6d25cd49059b 140 return true;
eencae 1:6d25cd49059b 141 } else {
eencae 1:6d25cd49059b 142 return false;
eencae 1:6d25cd49059b 143 }
eencae 1:6d25cd49059b 144 }
eencae 1:6d25cd49059b 145
eencae 1:6d25cd49059b 146 bool Gamepad::l_pressed()
eencae 1:6d25cd49059b 147 {
eencae 1:6d25cd49059b 148 // ISR must have been triggered
eencae 1:6d25cd49059b 149 if (l_flag) {
eencae 1:6d25cd49059b 150 l_flag = 0; // clear flag
eencae 1:6d25cd49059b 151 return true;
eencae 1:6d25cd49059b 152 } else {
eencae 1:6d25cd49059b 153 return false;
eencae 1:6d25cd49059b 154 }
eencae 0:a6288c29b936 155 }
eencae 1:6d25cd49059b 156
eencae 1:6d25cd49059b 157 bool Gamepad::r_pressed()
eencae 1:6d25cd49059b 158 {
eencae 1:6d25cd49059b 159 // ISR must have been triggered
eencae 1:6d25cd49059b 160 if (r_flag) {
eencae 1:6d25cd49059b 161 r_flag = 0; // clear flag
eencae 1:6d25cd49059b 162 return true;
eencae 1:6d25cd49059b 163 } else {
eencae 1:6d25cd49059b 164 return false;
eencae 1:6d25cd49059b 165 }
eencae 1:6d25cd49059b 166 }
eencae 1:6d25cd49059b 167
eencae 1:6d25cd49059b 168 bool Gamepad::back_pressed()
eencae 1:6d25cd49059b 169 {
eencae 1:6d25cd49059b 170 // ISR must have been triggered
eencae 1:6d25cd49059b 171 if (back_flag) {
eencae 1:6d25cd49059b 172 back_flag = 0; // clear flag
eencae 1:6d25cd49059b 173 return true;
eencae 1:6d25cd49059b 174 } else {
eencae 1:6d25cd49059b 175 return false;
eencae 1:6d25cd49059b 176 }
eencae 1:6d25cd49059b 177 }
eencae 1:6d25cd49059b 178
eencae 1:6d25cd49059b 179 bool Gamepad::start_pressed()
eencae 1:6d25cd49059b 180 {
eencae 1:6d25cd49059b 181 // ISR must have been triggered
eencae 1:6d25cd49059b 182 if (a_flag) {
eencae 1:6d25cd49059b 183 a_flag = 0; // clear flag
eencae 1:6d25cd49059b 184 return true;
eencae 1:6d25cd49059b 185 } else {
eencae 1:6d25cd49059b 186 return false;
eencae 1:6d25cd49059b 187 }
eencae 1:6d25cd49059b 188 }
eencae 3:964a6d95acdd 189
eencae 3:964a6d95acdd 190 bool Gamepad::joystick_pressed()
eencae 3:964a6d95acdd 191 {
eencae 3:964a6d95acdd 192 // ISR must have been triggered
eencae 3:964a6d95acdd 193 if (joy_flag) {
eencae 3:964a6d95acdd 194 joy_flag = 0; // clear flag
eencae 3:964a6d95acdd 195 return true;
eencae 3:964a6d95acdd 196 } else {
eencae 3:964a6d95acdd 197 return false;
eencae 3:964a6d95acdd 198 }
eencae 3:964a6d95acdd 199 }
eencae 3:964a6d95acdd 200
eencae 3:964a6d95acdd 201 // this method gets the magnitude of the joystick movement
eencae 3:964a6d95acdd 202 float Gamepad::get_mag()
eencae 3:964a6d95acdd 203 {
eencae 3:964a6d95acdd 204 Polar p = get_polar();
eencae 3:964a6d95acdd 205 return p.mag;
eencae 3:964a6d95acdd 206 }
eencae 3:964a6d95acdd 207
eencae 3:964a6d95acdd 208 // this method gets the angle of joystick movement (0 to 360, 0 North)
eencae 3:964a6d95acdd 209 float Gamepad::get_angle()
eencae 3:964a6d95acdd 210 {
eencae 3:964a6d95acdd 211 Polar p = get_polar();
eencae 3:964a6d95acdd 212 return p.angle;
eencae 3:964a6d95acdd 213 }
eencae 3:964a6d95acdd 214
eencae 4:bafb7f483e93 215 Direction Gamepad::get_direction()
eencae 3:964a6d95acdd 216 {
eencae 3:964a6d95acdd 217 float angle = get_angle(); // 0 to 360, -1 for centred
eencae 3:964a6d95acdd 218
eencae 3:964a6d95acdd 219 Direction d;
eencae 3:964a6d95acdd 220 // partition 360 into segments and check which segment the angle is in
eencae 3:964a6d95acdd 221 if (angle < 0.0f) {
eencae 3:964a6d95acdd 222 d = CENTRE; // check for -1.0 angle
eencae 3:964a6d95acdd 223 } else if (angle < 22.5f) { // then keep going in 45 degree increments
eencae 3:964a6d95acdd 224 d = N;
eencae 3:964a6d95acdd 225 } else if (angle < 67.5f) {
eencae 3:964a6d95acdd 226 d = NE;
eencae 3:964a6d95acdd 227 } else if (angle < 112.5f) {
eencae 3:964a6d95acdd 228 d = E;
eencae 3:964a6d95acdd 229 } else if (angle < 157.5f) {
eencae 3:964a6d95acdd 230 d = SE;
eencae 3:964a6d95acdd 231 } else if (angle < 202.5f) {
eencae 3:964a6d95acdd 232 d = S;
eencae 3:964a6d95acdd 233 } else if (angle < 247.5f) {
eencae 3:964a6d95acdd 234 d = SW;
eencae 3:964a6d95acdd 235 } else if (angle < 292.5f) {
eencae 3:964a6d95acdd 236 d = W;
eencae 3:964a6d95acdd 237 } else if (angle < 337.5f) {
eencae 3:964a6d95acdd 238 d = NW;
eencae 3:964a6d95acdd 239 } else {
eencae 3:964a6d95acdd 240 d = N;
eencae 3:964a6d95acdd 241 }
eencae 3:964a6d95acdd 242
eencae 3:964a6d95acdd 243 return d;
eencae 3:964a6d95acdd 244 }
eencae 3:964a6d95acdd 245
eencae 1:6d25cd49059b 246 ///////////////////// private methods ////////////////////////
eencae 1:6d25cd49059b 247
eencae 1:6d25cd49059b 248 void Gamepad::tone_off()
eencae 1:6d25cd49059b 249 {
eencae 1:6d25cd49059b 250 buzzer->write(0.0);
eencae 1:6d25cd49059b 251 }
eencae 1:6d25cd49059b 252
eencae 1:6d25cd49059b 253 void Gamepad::init_buttons()
eencae 1:6d25cd49059b 254 {
eencae 1:6d25cd49059b 255 // turn on pull-downs as other side of button is connected to 3V3
eencae 1:6d25cd49059b 256 // button is 0 when not pressed and 1 when pressed
eencae 1:6d25cd49059b 257 button_A->mode(PullDown);
eencae 1:6d25cd49059b 258 button_B->mode(PullDown);
eencae 1:6d25cd49059b 259 button_X->mode(PullDown);
eencae 1:6d25cd49059b 260 button_Y->mode(PullDown);
eencae 1:6d25cd49059b 261 button_back->mode(PullDown);
eencae 1:6d25cd49059b 262 button_start->mode(PullDown);
eencae 1:6d25cd49059b 263 button_L->mode(PullDown);
eencae 1:6d25cd49059b 264 button_R->mode(PullDown);
eencae 3:964a6d95acdd 265 button_joystick->mode(PullDown);
eencae 1:6d25cd49059b 266 // therefore setup rising edge interrupts
eencae 1:6d25cd49059b 267 button_A->rise(callback(this,&Gamepad::a_isr));
eencae 1:6d25cd49059b 268 button_B->rise(callback(this,&Gamepad::b_isr));
eencae 1:6d25cd49059b 269 button_X->rise(callback(this,&Gamepad::x_isr));
eencae 1:6d25cd49059b 270 button_Y->rise(callback(this,&Gamepad::y_isr));
eencae 1:6d25cd49059b 271 button_L->rise(callback(this,&Gamepad::l_isr));
eencae 1:6d25cd49059b 272 button_R->rise(callback(this,&Gamepad::r_isr));
eencae 1:6d25cd49059b 273 button_start->rise(callback(this,&Gamepad::start_isr));
eencae 1:6d25cd49059b 274 button_back->rise(callback(this,&Gamepad::back_isr));
eencae 3:964a6d95acdd 275 button_joystick->rise(callback(this,&Gamepad::joy_isr));
eencae 1:6d25cd49059b 276 }
eencae 1:6d25cd49059b 277
eencae 1:6d25cd49059b 278 // button interrupts ISRs
eencae 1:6d25cd49059b 279 void Gamepad::a_isr()
eencae 1:6d25cd49059b 280 {
eencae 1:6d25cd49059b 281 a_flag=1;
eencae 1:6d25cd49059b 282 }
eencae 1:6d25cd49059b 283 void Gamepad::b_isr()
eencae 1:6d25cd49059b 284 {
eencae 1:6d25cd49059b 285 b_flag=1;
eencae 1:6d25cd49059b 286 }
eencae 1:6d25cd49059b 287 void Gamepad::x_isr()
eencae 1:6d25cd49059b 288 {
eencae 1:6d25cd49059b 289 x_flag=1;
eencae 1:6d25cd49059b 290 }
eencae 1:6d25cd49059b 291 void Gamepad::y_isr()
eencae 1:6d25cd49059b 292 {
eencae 1:6d25cd49059b 293 y_flag=1;
eencae 1:6d25cd49059b 294 }
eencae 1:6d25cd49059b 295 void Gamepad::l_isr()
eencae 1:6d25cd49059b 296 {
eencae 1:6d25cd49059b 297 l_flag=1;
eencae 1:6d25cd49059b 298 }
eencae 1:6d25cd49059b 299 void Gamepad::r_isr()
eencae 1:6d25cd49059b 300 {
eencae 1:6d25cd49059b 301 r_flag=1;
eencae 1:6d25cd49059b 302 }
eencae 1:6d25cd49059b 303 void Gamepad::back_isr()
eencae 1:6d25cd49059b 304 {
eencae 1:6d25cd49059b 305 back_flag=1;
eencae 1:6d25cd49059b 306 }
eencae 1:6d25cd49059b 307 void Gamepad::start_isr()
eencae 1:6d25cd49059b 308 {
eencae 1:6d25cd49059b 309 start_flag=1;
eencae 3:964a6d95acdd 310 }
eencae 3:964a6d95acdd 311 void Gamepad::joy_isr()
eencae 3:964a6d95acdd 312 {
eencae 3:964a6d95acdd 313 joy_flag=1;
eencae 3:964a6d95acdd 314 }
eencae 3:964a6d95acdd 315
eencae 3:964a6d95acdd 316 // get raw joystick coordinate in range -1 to 1
eencae 3:964a6d95acdd 317 // Direction (x,y)
eencae 3:964a6d95acdd 318 // North (0,1)
eencae 3:964a6d95acdd 319 // East (1,0)
eencae 3:964a6d95acdd 320 // South (0,-1)
eencae 3:964a6d95acdd 321 // West (-1,0)
eencae 3:964a6d95acdd 322 Vector2D Gamepad::get_coord()
eencae 3:964a6d95acdd 323 {
eencae 3:964a6d95acdd 324 // read() returns value in range 0.0 to 1.0 so is scaled and centre value
eencae 3:964a6d95acdd 325 // substracted to get values in the range -1.0 to 1.0
eencae 3:964a6d95acdd 326 float x = 2.0f*( horiz->read() - _x0 );
eencae 3:964a6d95acdd 327 float y = 2.0f*( vert->read() - _y0 );
eencae 3:964a6d95acdd 328
eencae 3:964a6d95acdd 329 // Note: the x value here is inverted to ensure the positive x is to the
eencae 3:964a6d95acdd 330 // right. This is simply due to how the potentiometer on the joystick
eencae 3:964a6d95acdd 331 // I was using was connected up. It could have been corrected in hardware
eencae 3:964a6d95acdd 332 // by swapping the power supply pins. Instead it is done in software so may
eencae 3:964a6d95acdd 333 // need to be changed depending on your wiring setup
eencae 3:964a6d95acdd 334
eencae 3:964a6d95acdd 335 Vector2D coord = {-x,y};
eencae 3:964a6d95acdd 336 return coord;
eencae 3:964a6d95acdd 337 }
eencae 3:964a6d95acdd 338
eencae 3:964a6d95acdd 339 // This maps the raw x,y coord onto a circular grid.
eencae 3:964a6d95acdd 340 // See: http://mathproofs.blogspot.co.uk/2005/07/mapping-square-to-circle.html
eencae 3:964a6d95acdd 341 Vector2D Gamepad::get_mapped_coord()
eencae 3:964a6d95acdd 342 {
eencae 3:964a6d95acdd 343 Vector2D coord = get_coord();
eencae 3:964a6d95acdd 344
eencae 3:964a6d95acdd 345 // do the transformation
eencae 3:964a6d95acdd 346 float x = coord.x*sqrt(1.0f-pow(coord.y,2.0f)/2.0f);
eencae 3:964a6d95acdd 347 float y = coord.y*sqrt(1.0f-pow(coord.x,2.0f)/2.0f);
eencae 3:964a6d95acdd 348
eencae 3:964a6d95acdd 349 Vector2D mapped_coord = {x,y};
eencae 3:964a6d95acdd 350 return mapped_coord;
eencae 3:964a6d95acdd 351 }
eencae 3:964a6d95acdd 352
eencae 3:964a6d95acdd 353 // this function converts the mapped coordinates into polar form
eencae 3:964a6d95acdd 354 Polar Gamepad::get_polar()
eencae 3:964a6d95acdd 355 {
eencae 3:964a6d95acdd 356 // get the mapped coordinate
eencae 3:964a6d95acdd 357 Vector2D coord = get_mapped_coord();
eencae 3:964a6d95acdd 358
eencae 3:964a6d95acdd 359 // at this point, 0 degrees (i.e. x-axis) will be defined to the East.
eencae 3:964a6d95acdd 360 // We want 0 degrees to correspond to North and increase clockwise to 359
eencae 3:964a6d95acdd 361 // like a compass heading, so we need to swap the axis and invert y
eencae 3:964a6d95acdd 362 float x = coord.y;
eencae 3:964a6d95acdd 363 float y = coord.x;
eencae 3:964a6d95acdd 364
eencae 3:964a6d95acdd 365 float mag = sqrt(x*x+y*y); // pythagoras
eencae 3:964a6d95acdd 366 float angle = RAD2DEG*atan2(y,x);
eencae 3:964a6d95acdd 367 // angle will be in range -180 to 180, so add 360 to negative angles to
eencae 3:964a6d95acdd 368 // move to 0 to 360 range
eencae 3:964a6d95acdd 369 if (angle < 0.0f) {
eencae 3:964a6d95acdd 370 angle+=360.0f;
eencae 3:964a6d95acdd 371 }
eencae 3:964a6d95acdd 372
eencae 3:964a6d95acdd 373 // the noise on the ADC causes the values of x and y to fluctuate slightly
eencae 3:964a6d95acdd 374 // around the centred values. This causes the random angle values to get
eencae 3:964a6d95acdd 375 // calculated when the joystick is centred and untouched. This is also when
eencae 3:964a6d95acdd 376 // the magnitude is very small, so we can check for a small magnitude and then
eencae 3:964a6d95acdd 377 // set the angle to -1. This will inform us when the angle is invalid and the
eencae 3:964a6d95acdd 378 // joystick is centred
eencae 3:964a6d95acdd 379
eencae 3:964a6d95acdd 380 if (mag < TOL) {
eencae 3:964a6d95acdd 381 mag = 0.0f;
eencae 3:964a6d95acdd 382 angle = -1.0f;
eencae 3:964a6d95acdd 383 }
eencae 3:964a6d95acdd 384
eencae 3:964a6d95acdd 385 Polar p = {mag,angle};
eencae 3:964a6d95acdd 386 return p;
eencae 1:6d25cd49059b 387 }