Tom Rasmussen / Mbed 2 deprecated NucleoCube1

Dependencies:   BNO055_fusion_tom FastPWM mbed

Fork of NucleoCube1 by Will Church

cube.cpp@26:f2bb916262c9, 2017-04-13 (annotated)

Committer:
tomras12
Date:
Thu Apr 13 23:17:41 2017 +0000
Revision:
26:f2bb916262c9
Parent:
24:c7b3bac429c5
Child:
27:c3ca97f1db60
Having problems with loop timing

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 24:c7b3bac429c5 12
tomras12 24:c7b3bac429c5 13 void checkCalib(BNO055 *imu, Serial *pc) {
tomras12 24:c7b3bac429c5 14 pc->printf("Checking calibration status...\r\n");
tomras12 24:c7b3bac429c5 15 int stat = imu->read_calib_status();
tomras12 24:c7b3bac429c5 16 while(stat < 192) {
tomras12 24:c7b3bac429c5 17 pc->printf("Sys:%d Gyr:%d Acc:%d Mag:%d\r\n",
tomras12 24:c7b3bac429c5 18 (stat >> 6) & 0x03,
tomras12 24:c7b3bac429c5 19 (stat >> 4) & 0x03,
tomras12 24:c7b3bac429c5 20 (stat >> 2) & 0x03,
tomras12 24:c7b3bac429c5 21 stat & 0x03);
tomras12 24:c7b3bac429c5 22 wait_ms(250);
tomras12 24:c7b3bac429c5 23 stat = imu->read_calib_status();
tomras12 24:c7b3bac429c5 24 }
tomras12 24:c7b3bac429c5 25 pc->printf("Looks good buddy, put the cube down now.\r\n");
tomras12 26:f2bb916262c9 26 wait(2);
tomras12 24:c7b3bac429c5 27 }
tomras12 23:abe76b7c377a 28
tomras12 23:abe76b7c377a 29 /*
tomras12 23:abe76b7c377a 30 * Returns PWM duty cycle based on:
tomras12 23:abe76b7c377a 31 * - wv wheel velocity
tomras12 23:abe76b7c377a 32 * - ae angle error
tomras12 23:abe76b7c377a 33 * - bv body velocity
tomras12 23:abe76b7c377a 34 * - Kwv wheel vel gain
tomras12 23:abe76b7c377a 35 * - Kbt angle gain
tomras12 23:abe76b7c377a 36 * - Kbv body vel gain
tomras12 23:abe76b7c377a 37 */
tomras12 23:abe76b7c377a 38 double calcPWM(config *c)
tomras12 23:abe76b7c377a 39 {
tomras12 23:abe76b7c377a 40 // Converts and read the analog input value (value from 0.0 to 1.0):
tomras12 23:abe76b7c377a 41 double wv = c->hall->read();
tomras12 23:abe76b7c377a 42 wv = (wv - 2.0) * 5000.0; // Scale the velocity to rad/s
tomras12 23:abe76b7c377a 43
tomras12 23:abe76b7c377a 44 double bt = (*(c->angle) - c->eqAngle);
tomras12 23:abe76b7c377a 45
tomras12 23:abe76b7c377a 46 double r1 = (c->Kbt * bt + c->Kbv * *(c->vel) + c->Kwv * wv);
tomras12 23:abe76b7c377a 47
tomras12 23:abe76b7c377a 48 //Limit PWM range
tomras12 23:abe76b7c377a 49 if (r1 > 6.0) {r1 = 6.0;}
tomras12 23:abe76b7c377a 50 else if (r1 < -6.0) {r1 = -6.0;}
tomras12 23:abe76b7c377a 51
tomras12 23:abe76b7c377a 52 // Normalize for PWM output
tomras12 23:abe76b7c377a 53 r1 = ((.4*(r1/6.0)) + 0.5);
tomras12 23:abe76b7c377a 54
tomras12 23:abe76b7c377a 55 // Check if cube is too far tilted and send 0 torque
tomras12 23:abe76b7c377a 56 // May be redundant, check outer program
tomras12 23:abe76b7c377a 57 if (bt > (pi/8) || bt < -(pi/8)){
tomras12 23:abe76b7c377a 58 return .5;
tomras12 23:abe76b7c377a 59 }
tomras12 23:abe76b7c377a 60 return r1;
tomras12 23:abe76b7c377a 61 }
tomras12 23:abe76b7c377a 62
tomras12 23:abe76b7c377a 63 void updatePWM(config *c) {
tomras12 23:abe76b7c377a 64 c->pwm->write(calcPWM(c));
tomras12 23:abe76b7c377a 65 }
tomras12 23:abe76b7c377a 66