File content as of revision 2:983a818c6ddf:

#include <mbed.h>
#include "max32630fthr.h"
#include "bmi160.h"
#include "I2C.h"
#include "QEI.h"
#include "kalman.c"
#include "SDFileSystem.h"
#include <string>
#include "USBSerial.h"
 
//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);

// added by ken //
DigitalIn saveDataButton(P2_3);
SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // the pinout on the mbed Cool Components workshop board
Serial pc(USBTX, USBRX);
// added by ken - ends here //


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


bool debugModeOn = false;

/********************** 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++;
    }

    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);
}

/*********** Load and Update and Save Parameters ***************/ 
char inputMode = 'n';

void loadParam(){ //from SD
    //kp read
    FILE *fp_kp = fopen("/sd/mydir/param/kp.txt", "r");
    char Buf_p[30];    
    if(fp_kp == NULL) { error("Could not open file for reading\r\n"); }
    while(fgets (Buf_p, 30, fp_kp) != NULL){
        Buf_p[strlen(Buf_p)-1] = 0;
        printf("String = \"%s\" \r\n", Buf_p);
        }
    fclose(fp_kp);
    
    Kp = (float)atof(Buf_p);
    printf("Kp is loaded as %f. \r\n", Kp);
    
    //kd read
    FILE *fp_kd = fopen("/sd/mydir/param/kd.txt", "r");
    char Buf_d[30];    
    if(fp_kd == NULL) { error("Could not open file for reading\r\n"); }
    while(fgets (Buf_d, 30, fp_kd) != NULL){
        Buf_d[strlen(Buf_d)-1] = 0;
        printf("String = \"%s\" \r\n", Buf_d);
        }
    fclose(fp_kd);
    
    Kd = (float)atof(Buf_d);
    printf("Kd is loaded as %f. \r\n", Kd);
    
    //theta_d read
    FILE *fp_th = fopen("/sd/mydir/param/theta_d.txt", "r");
    char Buf_t[30];    
    if(fp_th == NULL) { error("Could not open file for reading\r\n"); }
    while(fgets (Buf_t, 30, fp_th) != NULL){
        Buf_t[strlen(Buf_t)-1] = 0;
        printf("String = \"%s\" \r\n", Buf_t);
        }
    fclose(fp_th);
    
    theta_d = (float)atof(Buf_t);
    printf("theta_d is loaded as %f. \r\n", fp_th);
    
}

void saveParam(){ //to SD

    // -- kp
    FILE *fp_kp = fopen("/sd/mydir/param/kp.txt", "w");
    if(fp_kp == NULL) {
        error("Could not open file for write\n");
    }
    char buf_p[10];
    sprintf(buf_p, "%.3f", Kp);
    fprintf(fp_kp, buf_p);
    fprintf(fp_kp, "\r\n");
    fclose(fp_kp); 
    printf("Kp saved as %f.\r\n", Kp);
    
    // -- kd
    FILE *fp_kd = fopen("/sd/mydir/param/kd.txt", "w");
    if(fp_kd == NULL) {
        error("Could not open file for write\n");
    }
    char buf_d[10];
    sprintf(buf_d, "%.3f", Kd);
    fprintf(fp_kd, buf_d);
    fprintf(fp_kd, "\r\n");
    fclose(fp_kd); 
    printf("Kd saved as %f.\r\n", Kd);
    
    // -- theta_d
    FILE *fp_th = fopen("/sd/mydir/param/theta_d.txt", "w");
    if(fp_th == NULL) {
        error("Could not open file for write\n");
    }
    char buf_t[10];
    sprintf(buf_t, "%.3f", theta_d);
    fprintf(fp_th, buf_t);
    fprintf(fp_th, "\r\n");
    fclose(fp_th); 
    printf("theta_d saved as %f.\r\n", theta_d);
    
}

char buffer_serial[20];
void serialUpdateVal(){
       
      pc.scanf("%s", &buffer_serial);
      pc.printf("I received ");
      pc.printf(buffer_serial);
      pc.printf("\n");
      
      if(inputMode == 'n'){
        if(buffer_serial[0] == 'p'){
            pc.printf("input mode is set to Kp, Please enter value. \n");
            inputMode = 'p';
         } if(buffer_serial[0] == 'd'){
            pc.printf("input mode is set to Kd, Please enter value. \n");
            inputMode = 'd';
         } if(buffer_serial[0] == 't'){
            pc.printf("input mode is set to theta_d, Please enter value. \n");
            inputMode = 't';
         } 
        } else if(inputMode == 'p'){
                Kp = (float)atof(buffer_serial);
                inputMode = 'n';
                pc.printf("Kp is set to %f \n\n", Kp);
                saveParam();
        } else if(inputMode == 'd'){
                Kd = (float)atof(buffer_serial);
                inputMode = 'n';
                pc.printf("Kd is set to %f \n\n", Kd);
                saveParam();
        } else if(inputMode == 't'){
                theta_d = (float)atof(buffer_serial);
                inputMode = 'n';
                pc.printf("theta_d is set to %f \n\n", theta_d);
                saveParam();
        }
    
        
        if(buffer_serial[0] == 'c'){
            pc.printf("Kp is %f | Kd is %f | theta_d is %f \n\n", Kp, Kd, theta_d);
            }
         
}

/******************** LogData to SD card  *****************************/

int trialTimeCount = 0;

const int logValNum = 4;
char logValName[logValNum][30];
float logVal[logValNum][10000];
int currentLogNum = 0;
 
void updateTrialTime(){
    //trial time read
    FILE *fp_tr = fopen("/sd/mydir/trialtime.txt", "r");
    
    char Buffer_t[512];
    
    if(fp_tr == NULL) { error("Could not open file for reading\r\n"); }
    
    while(fgets (Buffer_t, 512, fp_tr) != NULL){
        Buffer_t[strlen(Buffer_t)-1] = 0;
        printf("String = \"%s\" \r\n", Buffer_t);
        }
    fclose(fp_tr);
    
    trialTimeCount = (int)atof(Buffer_t);
    
    printf("last trialTimeCount was %i. \n", trialTimeCount);
    
    trialTimeCount++; //count up trial time
    printf("current trialTimeCount updated to %i. \n", trialTimeCount);
    
    FILE *fp3 = fopen("/sd/mydir/trialtime.txt", "w");
    if(fp3 == NULL) {
        error("Could not open file for write\n");
    }
    char buf[10];
    
    sprintf(buf, "%d", trialTimeCount);
    
    fprintf(fp3, buf);
    fprintf(fp3, "\r\n");
    fclose(fp3); 
    printf("trial time saved\n");
}

void logData(){ // log data time
    logVal[0][currentLogNum] = t.read();
    logVal[1][currentLogNum] = accX;
    logVal[2][currentLogNum] = accY;
    logVal[3][currentLogNum] = accZ;
    
    printf("logged data for %i. t.read() = %f \r\n", currentLogNum, t.read());
    
    currentLogNum++;   
}

void saveData(){ // call when the while loop ends or the button pressed
   sprintf( logValName[0], "time");
   sprintf( logValName[1], "accX");
   sprintf( logValName[2], "accY");
   sprintf( logValName[3], "accZ");
    
    char filename[256];
    sprintf(filename, "/sd/mydir/log/flipper_logData_%i.txt",trialTimeCount); 
    
    FILE *f_sv = fopen(filename, "w");
    if(f_sv == NULL) {
        error("Could not open file for write\n");
    }
    
    for(int i = 0; i < logValNum; i++){
        fprintf(f_sv, logValName[i]);
        fprintf(f_sv, ",");
    }
    fprintf(f_sv, "\r\n");
    
    for(int j = 0; j < currentLogNum; j++){
    for(int i = 0; i < logValNum; i++){
        //char buf_temp[10];
        //int a = 5;
        
        char cVal[8];  
 
         sprintf(cVal,"%.3f", logVal[i][j]);

        //sprintf(buf_temp, "%f", logVal[i][j]);
        
        fprintf(f_sv, cVal);
        fprintf(f_sv, ",");
    }
    fprintf(f_sv, "\r\n");
    }
    
    
    fclose(f_sv); 
    
    printf("Log Data file is saved as 'flipper_logData_%i.txt'.\n", trialTimeCount);
}

/*******************************************************************/



/******************** 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();
    
    float timeSinceCollision=0.0;

    // Run experiment
    while( t.read() < 10.0 && !debugModeOn) {
           //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);
                 
                 logData();
                 if(!saveDataButton){
                     saveData();
                     debugModeOn = true;
                 }
                 
            }
            motorPWM.write(0);
}




/****************************** MAIN *************************************/

int main()
{
    
    printf("test!!\n");
    updateTrialTime();
    loadParam();
    
    /* setup imu and kalman filter for rotation */
    setupIMU();
    setupKalman();

    //while( t.read() < 60.0) {
    /* run core program */
    executeMotorLoop();
    
    
    //}
    saveData();
    
    printf("Now you can set parameters by typing 'p', 'd', or 't'. You can check the values with 'c'.");
    while(1){ // debugMode
        serialUpdateVal();
    }

    /* 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;*/
}