Tom Rasmussen / Mbed 2 deprecated NucleoCube1

Dependencies:   BNO055_fusion_tom FastPWM mbed

Fork of NucleoCube1 by Will Church

cube.cpp@23:abe76b7c377a, 2017-04-12 (annotated)

Committer:
tomras12
Date:
Wed Apr 12 00:29:45 2017 +0000
Revision:
23:abe76b7c377a
Child:
24:c7b3bac429c5
Refactor to make code more adaptable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tomras12 23:abe76b7c377a 1 /*
tomras12 23:abe76b7c377a 2 * cube.h
tomras12 23:abe76b7c377a 3 * April 11, 2017
tomras12 23:abe76b7c377a 4 *
tomras12 23:abe76b7c377a 5 * Control software for balancing cube senior design project
tomras12 23:abe76b7c377a 6 *
tomras12 23:abe76b7c377a 7 * Will Church
tomras12 23:abe76b7c377a 8 * Tom Rasmussen
tomras12 23:abe76b7c377a 9 */
tomras12 23:abe76b7c377a 10
tomras12 23:abe76b7c377a 11 #include "cube.h"
tomras12 23:abe76b7c377a 12
tomras12 23:abe76b7c377a 13 /*
tomras12 23:abe76b7c377a 14 * Returns PWM duty cycle based on:
tomras12 23:abe76b7c377a 15 * - wv wheel velocity
tomras12 23:abe76b7c377a 16 * - ae angle error
tomras12 23:abe76b7c377a 17 * - bv body velocity
tomras12 23:abe76b7c377a 18 * - Kwv wheel vel gain
tomras12 23:abe76b7c377a 19 * - Kbt angle gain
tomras12 23:abe76b7c377a 20 * - Kbv body vel gain
tomras12 23:abe76b7c377a 21 */
tomras12 23:abe76b7c377a 22 double calcPWM(config *c)
tomras12 23:abe76b7c377a 23 {
tomras12 23:abe76b7c377a 24 // Converts and read the analog input value (value from 0.0 to 1.0):
tomras12 23:abe76b7c377a 25 double wv = c->hall->read();
tomras12 23:abe76b7c377a 26 wv = (wv - 2.0) * 5000.0; // Scale the velocity to rad/s
tomras12 23:abe76b7c377a 27
tomras12 23:abe76b7c377a 28 double bt = (*(c->angle) - c->eqAngle);
tomras12 23:abe76b7c377a 29
tomras12 23:abe76b7c377a 30 double r1 = (c->Kbt * bt + c->Kbv * *(c->vel) + c->Kwv * wv);
tomras12 23:abe76b7c377a 31
tomras12 23:abe76b7c377a 32 //Limit PWM range
tomras12 23:abe76b7c377a 33 if (r1 > 6.0) {r1 = 6.0;}
tomras12 23:abe76b7c377a 34 else if (r1 < -6.0) {r1 = -6.0;}
tomras12 23:abe76b7c377a 35
tomras12 23:abe76b7c377a 36 // Normalize for PWM output
tomras12 23:abe76b7c377a 37 r1 = ((.4*(r1/6.0)) + 0.5);
tomras12 23:abe76b7c377a 38
tomras12 23:abe76b7c377a 39 // Check if cube is too far tilted and send 0 torque
tomras12 23:abe76b7c377a 40 // May be redundant, check outer program
tomras12 23:abe76b7c377a 41 if (bt > (pi/8) || bt < -(pi/8)){
tomras12 23:abe76b7c377a 42 return .5;
tomras12 23:abe76b7c377a 43 }
tomras12 23:abe76b7c377a 44 return r1;
tomras12 23:abe76b7c377a 45 }
tomras12 23:abe76b7c377a 46
tomras12 23:abe76b7c377a 47 void updatePWM(config *c) {
tomras12 23:abe76b7c377a 48 c->pwm->write(calcPWM(c));
tomras12 23:abe76b7c377a 49 }
tomras12 23:abe76b7c377a 50