Diff: main.cpp

Revision:

1:59124c69d0c3

Parent:

0:73a369b7b5b4

Child:

2:983a818c6ddf

--- a/main.cpp	Sat Dec 02 06:13:14 2017 +0000
+++ b/main.cpp	Sat Dec 02 21:23:00 2017 +0000
@@ -1,41 +1,477 @@
-#include "mbed.h"
+#include <mbed.h>
+#include "max32630fthr.h"
+#include "bmi160.h"
+#include "I2C.h"
+#include "QEI.h"
+#include "kalman.c"
 #include "SDFileSystem.h"
  
-SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // the pinout on the mbed Cool Components workshop board
- 
-FILE * fp2;
-char Buffer[512];
-char * pEnd;
-float Temperature, Humidity;
- 
-int main() {
- 
-    ////// Writing to SD card
-    printf("Hello World!\n");   
- 
-    mkdir("/sd/mydir", 0777);
-    
-    FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
-    if(fp == NULL) {
-        error("Could not open file for write\n");
+//Defining PI
+#ifndef M_PI
+#define M_PI 3.1415
+#endif
+
+PwmOut motorPWM(P4_0);        // Motor PWM output
+DigitalOut motorFwd(P5_6);    // Motor forward enable
+DigitalOut motorRev(P5_5);    // Motor backward enable
+
+//Analog input
+AnalogIn   ain(AIN_5);
+Timer t;                    // Timer to measure elapsed time of experiment
+QEI encoder(P5_3,P5_4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
+
+DigitalOut led1(LED1, 1);
+
+MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
+I2C i2cBus_acc(P5_7, P6_0);
+BMI160_I2C* m_imu;//(i2cBus_acc, BMI160_I2C::I2C_ADRS_SDO_LO);
+BMI160::AccConfig accConfig;
+BMI160::GyroConfig gyroConfig;
+uint32_t failures = 0;
+
+// for storing values
+float imuTemperature;
+BMI160::SensorData accData;
+BMI160::SensorData gyroData;
+BMI160::SensorTime sensorTime;
+
+// IMU Methods
+float accX = 0.0;
+float accY = 0.0;
+float accZ = 0.0;
+
+float gyroX = 0.0;
+float gyroY = 0.0;
+float gyroZ = 0.0;
+
+// Collision Detection Variables - originally only for single collisions
+bool bool_collision = false; // collision state
+float collisionTime = 0.0; //time collision occurred
+float collisionTimeRemaining = 0.0; //variable to hold bool_collision for set time
+float collisionDuration = 0.5; // set as variable denoted collision duration.
+                              // the duration of actuation
+
+/****************** MOTOR CONTROL VARIABLES FOR STATES  **********************/
+//For motor control
+//Variables for states
+float theta_d = 0.0;
+//float Kp = 0.0;
+float Kd = 1.2; //made up
+float Ki = 0.1; //made up
+
+//Declare Parameters
+float e = 0.0; //error, theta_d-theta
+float theta = 0.0; //present theta
+float theta_dot = 0.0; //present angular velocity
+float val = 0.0; //
+
+float lastTime = 0.0; //last time step
+float presentTime = 0.0; //current time step
+float dT = 0.0; //time difference
+
+/** Set gain parameters using bluetooth **/
+//Default Gains set to avoid problems
+float i_d = 0; //target current % changed by code
+float Kp = 3.0; // proportional gain
+float r = 3.2; // winding resistance;
+float k_b = 0.23; // back emf
+float k_th = 4; // proportional theta gain for torque
+float b_th = 0.2185; // proportional theta_dot gain for damping
+float v_th = 0.0185;
+
+float i = 0.0; // present current
+float e_accum = 0.0;
+
+
+float torque = 0.0; //
+float outputVal;//final compensation value (in rad) --> converted into dutycycle
+
+/********************** FEEDBACK UTILITY FUNCTIONS ******************************/
+// A min function (as in min(a,b) or max(a,b)))
+float min(float a, float b){
+    return a<b ? a : b;
+}
+
+float max(float a, float b){
+    return a>b ? a : b;
+}
+// for final angle to pwm duty cycle calc
+float toDutyCycle(float inputRad){
+    float temp = abs(inputRad);
+    if (temp>12){
+        return 1.0;
+    }else{
+        return temp/12.0;
+    }
+    //abs(min(12.0,inputRad)/12.0);
+}
+// Calculates angles in radians from position
+float getAngleFromPulses(int input){
+    return input*(2*M_PI)/1200.0;
+}
+// Set motor direction
+void setMotorDir(char dir){
+    if (dir=='f'){
+        motorFwd = 1;
+        motorRev = 0;
+    }else{
+        motorFwd = 0;
+        motorRev = 1;
+    }
+}
+
+// gets current
+float readCurrent(){
+  return 36.7*ain.read()*(1.815)-18.3;
+  // return 36.7*ain.read()*(1.815)-18.3;
+}
+
+/********************* LED DISPLAY UTILITY FUNCTIONS *************************/
+PwmOut rLED(LED1);
+PwmOut gLED(LED2);
+PwmOut bLED(LED3);
+
+void updateLEDS(float b1,float b2,float b3){
+
+  if(!bool_collision){
+    rLED.write(max(1-b1/4,0.0));
+    gLED.write(max(1-b2/4,0.0));
+    bLED.write(max(1-b3/4,0.0));
+  }else{
+    rLED.write(0);
+    gLED.write(0);
+    bLED.write(1);
+  }
+}
+
+/********************* COLLISION UTILITY FUNCTIONS *************************/
+float last_xAcc = 0.0;
+
+bool checkCollision(float xAccel,float dT){
+  //printf("%f\n",xAccel);
+  //if ((xAccel > 1.85) || (xAccel <-1.85) || (collisionTimeRemaining>0)){
+  //if (last_xAcc<0 && xAccel>0){
+    float floatme = (xAccel-last_xAcc)/dT;
+    if (floatme>200.0){
+   //printf("Collision! %f\n",xAccel);
+    collisionTimeRemaining = max((collisionDuration)-(t.read()-collisionTime),0.0);
+   /* if ((xAccel>1.85) || xAccel <-1.85){
+      collisionTimeRemaining = collisionDuration;
+    }*/
+    return true;
+  }else{
+    return false;
+  }
+}
+
+//unused function for now
+float collisionActuation(float duration){
+  //first decide start time
+  //then actuate for a set duration
+  return duration;
+}
+
+
+
+/*****************************************************************************/
+
+/******************** SETUP IMU *****************************/
+kalman filterRotation;
+double refAngle = 0.0;
+double qGyro = 0.0;
+double qAngle = 0.0;
+
+void setupKalman(){
+    kalman_init(&filterRotation, R_matrix,Q_Gyro_matrix,Q_Accel_matrix);
+}
+
+
+void setupIMU()
+{
+    i2cBus_acc.frequency(400000);
+    m_imu = new BMI160_I2C(i2cBus_acc, BMI160_I2C::I2C_ADRS_SDO_LO);
+    printf("\033[H");  //home
+    printf("\033[0J");  //erase from cursor to end of screen
+    if(m_imu->setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
+        printf("Failed to set gyroscope power mode\n");
+        failures++;
+    }
+    wait_ms(100);
+
+    if(m_imu->setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
+        printf("Failed to set accelerometer power mode\n");
+        failures++;
+    }
+    wait_ms(100);
+
+
+    //example of setting user defined configuration
+    accConfig.range = BMI160::SENS_4G;
+    accConfig.us = BMI160::ACC_US_OFF;
+    accConfig.bwp = BMI160::ACC_BWP_2;
+    accConfig.odr = BMI160::ACC_ODR_12; //reads at 160 kHz
+
+    if(m_imu->getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) {
+        printf("ACC Range = %d\n", accConfig.range);
+        printf("ACC UnderSampling = %d\n", accConfig.us);
+        printf("ACC BandWidthParam = %d\n", accConfig.bwp);
+        printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
+    } else {
+        printf("Failed to get accelerometer configuration\n");
+        failures++;
+    }
+
+    if(m_imu->getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) {
+            printf("GYRO Range = %d\n", gyroConfig.range);
+            printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
+            printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
+    } else {
+        printf("Failed to get gyroscope configuration\n");
+        failures++;
     }
-    fprintf(fp, "Hello fun SD Card World!\n");
-    fprintf(fp, "Dan says hello\n");
-    fprintf(fp, "Ken says hello\n");
-    fprintf(fp, "Grace says hello\n");
-    fclose(fp); 
- 
-    printf("Goodbye World!\n");
-    
-    ///// Reading from SD card
+
+    wait(1.0);
+
+    m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
+    m_imu->getTemperature(&imuTemperature);
+
+    accX = accData.xAxis.scaled;
+    accY = accData.yAxis.scaled;
+    accZ = accData.zAxis.scaled;
+
+    printf("ACC xAxis = %s%4.3f\n", "\033[K", accX);
+    printf("ACC xAxis = %s%4.3f\n", "\033[K", accY);
+    printf("ACC xAxis = %s%4.3f\n", "\033[K", accZ);
+
+
+    updateLEDS(accX,accY,accZ);
+}
+
+/******************** READ IMU *****************************/
+
+// ▯  x is up, y is left, z is out of the board (towards me). BLE module is on top.
+// R i = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
+void getKalmanPrediction(double dT, float gyroReading, float accReading, float R){
+    kalman_predict(&filterRotation, gyroReading,  dT);
+    kalman_update(&filterRotation, acos(accReading/R));
+}
+
+
+float lastKalTime = 0.0;
+float R = 0.0;
+
+void readIMU()
+{
+    m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
+    m_imu->getTemperature(&imuTemperature);
+
+    //printf("ACC xAxis = %s%4.3f\n", "\033[K", accData.xAxis.scaled);
+    //printf("ACC yAxis = %s%4.3f\n", "\033[K", accData.yAxis.scaled);
+    //printf("ACC zAxis = %s%4.3f\n\n", "\033[K", accData.zAxis.scaled);
+
+    //    printf("GYRO xAxis = %s%5.1f\n", "\033[K", gyroData.xAxis.scaled);
+    //    printf("GYRO yAxis = %s%5.1f\n", "\033[K", gyroData.yAxis.scaled);
+    //    printf("GYRO zAxis = %s%5.1f\n\n", "\033[K", gyroData.zAxis.scaled);
+
+    //    printf("Sensor Time = %s%f\n", "\033[K", sensorTime.seconds);
+    //    printf("Sensor Temperature = %s%5.3f\n", "\033[K", imuTemperature);
+
+    accX = accData.xAxis.scaled;
+    accY = accData.yAxis.scaled;
+    accZ = accData.zAxis.scaled;
+
+    gyroX = gyroData.xAxis.scaled;
+    gyroY = gyroData.yAxis.scaled;
+    gyroZ = gyroData.zAxis.scaled;
+
+    //printf("%4.3f\n", accX);
+
+    //printf("ACC xAxis = %s%4.3f\n", "\033[K", accX);
+    //printf("ACC yAxis = %s%4.3f\n", "\033[K", accY);
+    //printf("ACC zAxis = %s%4.3f\n\n", "\033[K", accZ);
+    R = sqrt(std::pow(accX, 2) + std::pow(accY, 2) + std::pow(accZ, 2));
+        getKalmanPrediction(t.read()-lastKalTime, gyroY, accY, R);
+        //printf("%f\n",kalman_get_angle(&filterRotation));
+        lastKalTime=t.read();
+
+    updateLEDS(accX,accY,accZ);
+}
+/*******************************************************************/
+
+/******************** EXECUTE MOTOR LOOP *****************************/
+
+float pikeAngle = 0.0;
+float hitTimes = 0;
+
+/** Core Motor Code Loop**/
+void executeMotorLoop(){
+
+    printf("Entering Motor Loop\n");
+    // initial setup
+    t.reset();
+    t.start();
+    encoder.reset();
+    setMotorDir('f'); //begin with motor driving forward
+    motorPWM.write(0);
+    lastTime=t.read();
     
-    fp2 = fopen("/sd/mydir/sdtest.txt", "r");
-    if(fp2 == NULL) { error("Could not open file for reading\r\n"); }
-    
-    while(fgets (Buffer, 512, fp2) != NULL){
-        Buffer[strlen(Buffer)-1] = 0;
-        printf("String = \"%s\" \r\n", Buffer);
-        }
-    fclose(fp2);
-    
+    float timeSinceCollision=0.0;
+
+    // Run experiment
+    while( t.read() < 60.0) {
+           //Update IMU and Kalman
+            readIMU();
+
+            //R = sqrt(std::pow(accX, 2) + std::pow(accY, 2) + std::pow(accZ, 2));
+            //getKalmanPrediction(t.read()-lastTime, gyroY, accY, R);
+            //printf("kalman angle: %f\n",kalman_get_angle(&filterRotation));
+
+            //read present time and calculate dt
+            presentTime = t.read();
+            dT = presentTime - lastTime;
+
+            float floatme = (accX-last_xAcc)/dT;
+
+            //printf("last %f\n",last_xAcc);
+            //printf("now %f\n", accX);
+            //printf("%f\n",floatme);
+            lastTime = presentTime;
+
+            // Perform control loop logic
+                theta = getAngleFromPulses(encoder.getPulses());
+                theta_dot = getAngleFromPulses(encoder.getVelocity());
+
+                //check collisions
+
+                //If new hit. Otherwise bool_collision will do the work.
+                if ((!bool_collision) & checkCollision(accX,dT)){
+                  collisionTime = t.read();
+                  collisionTimeRemaining = collisionDuration;
+                  bool_collision = true;
+                  hitTimes = hitTimes + 1;
+                }else{
+                  //see if actuation is maintained:
+                  bool_collision = checkCollision(accX,dT);
+                }
+
+                //originally default state (drop)
+                //torque is defaulty 0
+                torque = 0.0;
+                
+                // state when in stance (bool collision)
+                 if (bool_collision){
+                    torque = -0.2;
+                    i_d = torque/k_b;
+
+                     //Update Error and accumalated error term
+                    i = readCurrent();
+                    e = i_d-i;
+                    outputVal = r*i_d+Kp*(e)-k_b*theta_dot;
+                    timeSinceCollision = t.read();
+                }
+                
+                // state in the air (after bool collision duration off)
+                else{
+                    
+                    if (hitTimes >1){
+                        if ((t.read()-timeSinceCollision)<0.3){
+                            theta_d = -0.3;
+                            }
+                        if (  ((t.read()-timeSinceCollision)>0.3) && ((t.read()-timeSinceCollision)<0.7) ){
+                            theta_d = 0.31;
+                            }
+                        if ((t.read()-timeSinceCollision)>=1.0){
+                            theta_d = 0.64;
+                            }
+                            
+                            //at hit -1.7  t=-.3
+                            //peak -0.7    t=.31
+                            //end is 0.2   t=.64
+                            e = theta_d-theta;
+                            //e_accum = e_accum+e*dT;
+                            Kp = 5;
+                            Kd = 0.5;
+                            outputVal = Kp*e + Kd*(-theta_dot);
+                        }
+                }
+                
+                if (hitTimes<=1){
+                    outputVal = 0;    
+                }
+                printf("%f",hitTimes);
+
+                
+            
+
+
+                //direct input for torque
+         /*       if (bool_collision){
+                    torque = 1.4;
+                }else{
+                    torque = 0.0;
+                }*/
+                last_xAcc = accX;
+                //printf("HEY\n");
+
+                //set reference current for torque
+                //theta_d = 0.0;//M_PI; // target at 180 degrees;
+                //torque = -k_th*(theta_d-theta)-b_th*-theta_dot+v_th*-theta_dot;
+             
+
+               // printf("currentError: %f\n",e);
+                //temp removal of force for non collision
+                //printf("%f\n",ain.read());
+
+                
+
+               
+
+               //regular pid
+                //Update Error and accumalated error term
+                /*e = theta_d-theta;
+                e_accum = e_accum+e*dT;
+                outputVal = Kp*e + Kd*(-theta_dot) + Ki*e_accum;*/
+
+               if (outputVal<0){
+                //negative difference, need to move motor backward to correct
+                    setMotorDir('f');
+                }
+                if (outputVal>0){
+                //positive difference, need to move motor forward to correct
+                    setMotorDir('r');
+                }
+                 motorPWM.write(toDutyCycle(abs(outputVal)));
+                 //printf("Serial: %f\n",outputVal);
+            }
+            motorPWM.write(0);
 }
+
+/****************************** MAIN *************************************/
+
+int main()
+{
+    /* setup imu and kalman filter for rotation */
+    setupIMU();
+    setupKalman();
+
+    /* run core program */
+    executeMotorLoop();
+
+
+    /* extra code for threading accelerometer kalman data */
+    //Thread eventThread;
+    //eventQueue.call_every(0.00001, periodicIMUCallback);
+    //eventQueue.dispatch_forever();
+    //eventThread.start(callback(&eventQueue, &EventQueue::dispatch_forever));
+
+
+    /*eventQueue.call_every(0.2, periodicCallback);
+
+    BLE &ble = BLE::Instance();
+    ble.onEventsToProcess(scheduleBleEventsProcessing);
+    ble.init(bleInitComplete);
+    /*eventQueue.call_every(0.2, periodicCallback);
+    eventQueue.dispatch_forever();
+
+    return 0;*/
+}
\ No newline at end of file