main.cpp

00001 #include <mbed.h>
00002 #include "max32630fthr.h"
00003 #include "bmi160.h"
00004 #include "I2C.h"
00005 #include "QEI.h"
00006 #include "kalman.c"
00007 #include "SDFileSystem.h"
00008 #include <string>
00009 #include "USBSerial.h"
00010 
00011 //Defining PI
00012 #ifndef M_PI
00013 #define M_PI 3.1415
00014 #endif
00015 
00016 float hitTimes = 0;
00017 /********************** PIN DEFINITIONS ****************************/
00018 PwmOut motorPWM(P4_0);        // Motor PWM output
00019 DigitalOut motorFwd(P5_6);    // Motor forward enable
00020 DigitalOut motorRev(P5_5);    // Motor backward enable
00021 
00022 //Analog input
00023 AnalogIn   ain(AIN_5);
00024 Timer t;                    // Timer to measure elapsed time of experiment
00025 QEI encoder(P5_3,P5_4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
00026 
00027 DigitalOut led1(LED1, 1);
00028 
00029 // added by ken //
00030 DigitalIn saveDataButton(P2_3);
00031 SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // the pinout on the mbed Cool Components workshop board
00032 Serial pc(USBTX, USBRX);
00033 // added by ken - ends here //
00034 
00035 /********************INSTANTIATING BOARD and SENSORS *************************/
00036 //Board Initialization
00037 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
00038 
00039 // IMU Initialization
00040 I2C i2cBus_acc(P5_7, P6_0);
00041 BMI160_I2C* m_imu;
00042 BMI160::AccConfig accConfig;
00043 BMI160::GyroConfig gyroConfig;
00044 uint32_t failures = 0;
00045 
00046 /************** VALUES FOR STORING IMU and KALMAN ANGLE **********************/
00047 
00048 // IMU Variables
00049 float imuTemperature;
00050 BMI160::SensorData accData;
00051 BMI160::SensorData gyroData;
00052 BMI160::SensorTime sensorTime;
00053 float accX = 0.0;
00054 float accY = 0.0;
00055 float accZ = 0.0;
00056 float gyroX = 0.0;
00057 float gyroY = 0.0;
00058 float gyroZ = 0.0;
00059 
00060 // Kalman Angle
00061 float kalmanAngle = 0.0;
00062 
00063 // Collision Detection Variables - originally only for single collisions
00064 bool bool_collision = false; // collision state
00065 float collisionTime = 0.0; //time collision occurred
00066 float collisionTimeRemaining = 0.0; //variable to hold bool_collision for set time
00067 float collisionDuration = 0.2; // duration of collision (rough measured experimentally)
00068 
00069 
00070 /****************** MOTOR CONTROL VARIABLES FOR STATES  **********************/
00071 //For motor control
00072 //Variables for states
00073 float theta_d = 0.0;
00074 float Kp = 3.0; // proportional gain
00075 float Kd = 1.2; //made up
00076 float Ki = 0.1; //made up
00077 
00078 //Declare Parameters
00079 float e = 0.0; //error, theta_d-theta
00080 float theta = 0.0; //present theta
00081 float theta_dot = 0.0; //present angular velocity
00082 float val = 0.0; //
00083 
00084 float lastTime = 0.0; //last time step
00085 float presentTime = 0.0; //current time step
00086 float dT = 0.045; //time difference
00087 
00088 /** Set gain parameters by log **/
00089 //Default Gains set to avoid problems
00090 float i_d = 0; //target current % changed by code
00091 float r = 3.2; // winding resistance;
00092 float k_b = 0.23; // back emf
00093 
00094 float k_th = 4; // proportional theta gain for torque
00095 float b_th = 0.2185; // proportional theta_dot gain for damping
00096 float v_th = 0.0185; //viscous friction coefficient (from experiments)
00097 
00098 float torqueCol_d = 0.2; //torque target for collision
00099 float torqueFlight_d = 0.0; //torque target for flight
00100 
00101 float i = 0.0; // present current
00102 float e_accum = 0.0;
00103 
00104 float torque = 0.0; //present torque
00105 float outputVal;//final compensation value (in rad) --> converted into dutycycle
00106 
00107 /********************** ACTUATION STRATEGY INDICATOR ***************************/
00108 float flipMode = 1; //defaulty starts with stiffness strategy
00109 
00110 /********************** DEBUG MODE VARIABLES ***********************************/
00111 bool debugModeOn = false;
00112 
00113 
00114 /********************** FEEDBACK UTILITY FUNCTIONS ******************************/
00115 // A min function (as in min(a,b) or max(a,b)))
00116 float min(float a, float b){
00117     return a<b ? a : b;
00118 }
00119 
00120 float max(float a, float b){
00121     return a>b ? a : b;
00122 }
00123 // for final angle to pwm duty cycle calc
00124 float toDutyCycle(float inputRad){
00125     float temp = abs(inputRad);
00126     if (temp>12){
00127         return 1.0;
00128     }else{
00129         return temp/12.0;
00130     }
00131     //abs(min(12.0,inputRad)/12.0);
00132 }
00133 // Calculates angles in radians from position
00134 float getAngleFromPulses(int input){
00135     return input*(2*M_PI)/1200.0;
00136 }
00137 // Set motor direction
00138 void setMotorDir(char dir){
00139     if (dir=='f'){
00140         motorFwd = 1;
00141         motorRev = 0;
00142     }else{
00143         motorFwd = 0;
00144         motorRev = 1;
00145     }
00146 }
00147 
00148 // gets current
00149 float readCurrent(){
00150   return 36.7*ain.read()*(1.815)-18.3;
00151   // return 36.7*ain.read()*(1.815)-18.3;
00152 }
00153 
00154 /********************* LED DISPLAY UTILITY FUNCTIONS *************************/
00155 PwmOut rLED(LED1);
00156 PwmOut gLED(LED2);
00157 PwmOut bLED(LED3);
00158 
00159 void updateLEDS(float b1,float b2,float b3){
00160 
00161   if(!bool_collision){
00162     rLED.write(max(1-b1/4,0.0));
00163     gLED.write(max(1-b2/4,0.0));
00164     bLED.write(max(1-b3/4,0.0));
00165   }else{
00166     rLED.write(0);
00167     gLED.write(0);
00168     bLED.write(1);
00169   }
00170 }
00171 
00172 /********************* COLLISION UTILITY FUNCTIONS *************************/
00173 float last_xAcc = 0.0;
00174 float jerk = 0.0;
00175 bool smashHappened = false;
00176 
00177 bool checkCollision(float xAccel,float dT){
00178     jerk = (xAccel-last_xAcc)/dT;
00179     printf("%f\n",(xAccel-last_xAcc));
00180     if ((xAccel-last_xAcc)<-1.5){//(jerk>100){ 
00181      return true;
00182      }
00183      else{
00184       if (collisionTimeRemaining >0.0){
00185           return true;
00186       }else{
00187       return false;
00188      }
00189     }
00190 }
00191 
00192 //unused function for now
00193 float collisionActuation(float duration){
00194   //first decide start time
00195   //then actuate for a set duration
00196   return duration;
00197 }
00198 
00199 
00200 
00201 /*****************************************************************************/
00202 
00203 /******************** SETUP IMU *****************************/
00204 kalman filterRotation;
00205 double refAngle = 0.0;
00206 double qGyro = 0.0;
00207 double qAngle = 0.0;
00208 
00209 void setupKalman(){
00210     kalman_init(&filterRotation, R_matrix,Q_Gyro_matrix,Q_Accel_matrix);
00211 }
00212 
00213 
00214 void setupIMU()
00215 {
00216     i2cBus_acc.frequency(400000);
00217     m_imu = new BMI160_I2C(i2cBus_acc, BMI160_I2C::I2C_ADRS_SDO_LO);
00218     printf("\033[H");  //home
00219     printf("\033[0J");  //erase from cursor to end of screen
00220     if(m_imu->setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
00221         printf("Failed to set gyroscope power mode\n");
00222         failures++;
00223     }
00224     wait_ms(100);
00225 
00226     if(m_imu->setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
00227         printf("Failed to set accelerometer power mode\n");
00228         failures++;
00229     }
00230     wait_ms(100);
00231 
00232 
00233     //example of setting user defined configuration
00234     accConfig.range = BMI160::SENS_4G;
00235     accConfig.us = BMI160::ACC_US_OFF;
00236     accConfig.bwp = BMI160::ACC_BWP_2;
00237     accConfig.odr = BMI160::ACC_ODR_12; //reads at 160 kHz
00238 
00239     if(m_imu->getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) {
00240         printf("ACC Range = %d\n", accConfig.range);
00241         printf("ACC UnderSampling = %d\n", accConfig.us);
00242         printf("ACC BandWidthParam = %d\n", accConfig.bwp);
00243         printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
00244     } else {
00245         printf("Failed to get accelerometer configuration\n");
00246         failures++;
00247     }
00248 
00249     if(m_imu->getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) {
00250             printf("GYRO Range = %d\n", gyroConfig.range);
00251             printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
00252             printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
00253     } else {
00254         printf("Failed to get gyroscope configuration\n");
00255         failures++;
00256     }
00257 
00258     wait(1.0);
00259 
00260     m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
00261     m_imu->getTemperature(&imuTemperature);
00262 
00263     accX = accData.xAxis.scaled;
00264     accY = accData.yAxis.scaled;
00265     accZ = accData.zAxis.scaled;
00266 
00267     printf("ACC xAxis = %s%4.3f\n", "\033[K", accX);
00268     printf("ACC xAxis = %s%4.3f\n", "\033[K", accY);
00269     printf("ACC xAxis = %s%4.3f\n", "\033[K", accZ);
00270 
00271 
00272     updateLEDS(accX,accY,accZ);
00273 }
00274 
00275 /******************** READ IMU *****************************/
00276 
00277 // ▯  x is up, y is left, z is out of the board (towards me). BLE module is on top.
00278 // R i = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
00279 void getKalmanPrediction(double dT, float gyroReading, float accReading, float R){
00280     kalman_predict(&filterRotation, gyroReading,  dT);
00281     kalman_update(&filterRotation, acos(accReading/R));
00282 }
00283 
00284 
00285 float lastKalTime = 0.0;
00286 float R = 0.0;
00287 float startTimeReference = 0.0;
00288 
00289 void readIMU()
00290 {
00291     m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
00292     m_imu->getTemperature(&imuTemperature);
00293 
00294     accX = accData.xAxis.scaled;
00295     accY = accData.yAxis.scaled;
00296     accZ = accData.zAxis.scaled;
00297 
00298     gyroX = gyroData.xAxis.scaled;
00299     gyroY = gyroData.yAxis.scaled;
00300     gyroZ = gyroData.zAxis.scaled;
00301 
00302     R = sqrt(std::pow(accX, 2) + std::pow(accY, 2) + std::pow(accZ, 2));
00303     getKalmanPrediction(t.read()-lastKalTime, gyroX, accX, R);
00304     //printf("%f\n",kalman_get_angle(&filterRotation));
00305     kalmanAngle = kalman_get_angle(&filterRotation);
00306 
00307     lastKalTime=t.read();
00308 
00309     updateLEDS(accX,accY,accZ);
00310 }
00311 
00312 /*********** Load and Update and Save Parameters ***************/
00313 char inputMode = 'n';
00314 
00315 void loadParam(){ //from SD
00316   char buf[15];
00317 
00318   // 0 -- collisionDuration read
00319   FILE *fp = fopen("/sd/mydir/param/collisionDuration.txt", "r");
00320   memset(buf, '\0', strlen(buf));
00321   if(fp == NULL) { error("Could not open file for reading\r\n"); }
00322   while(fgets (buf, 15, fp) != NULL){
00323       buf[strlen(buf)-1] = 0;
00324       }
00325   fclose(fp);
00326   collisionDuration = (float)atof(buf);
00327   printf("collisionDuration is loaded as %f. \r\n", collisionDuration);
00328 
00329     // 1 -- kp read
00330     fp = fopen("/sd/mydir/param/kp.txt", "r");
00331     memset(buf, '\0', strlen(buf));
00332     if(fp == NULL) { error("Could not open file for reading\r\n"); }
00333     while(fgets (buf, 15, fp) != NULL){
00334         buf[strlen(buf)-1] = 0;
00335         }
00336     fclose(fp);
00337     Kp = (float)atof(buf);
00338     printf("Kp is loaded as %f. \r\n", Kp);
00339 
00340     // 2 -- kd read
00341     fp = fopen("/sd/mydir/param/kd.txt", "r");
00342     memset(buf, '\0', strlen(buf));
00343     if(fp == NULL) { error("Could not open file for reading\r\n"); }
00344     while(fgets (buf, 15, fp) != NULL){
00345         buf[strlen(buf)-1] = 0;
00346         }
00347     fclose(fp);
00348     Kd = (float)atof(buf);
00349     printf("Kd is loaded as %f. \r\n", Kd);
00350 
00351     // 3 -- theta_d read
00352     fp = fopen("/sd/mydir/param/theta_d.txt", "r");
00353     memset(buf, '\0', strlen(buf));
00354     if(fp == NULL) { error("Could not open file for reading\r\n"); }
00355     while(fgets (buf, 15, fp) != NULL){
00356         buf[strlen(buf)-1] = 0;
00357         }
00358     fclose(fp);
00359     theta_d = (float)atof(buf);
00360     printf("theta_d is loaded as %f. \r\n", theta_d);
00361 
00362     // 4 -- k_th read
00363     fp = fopen("/sd/mydir/param/k_th.txt", "r");
00364     memset(buf, '\0', strlen(buf));
00365     if(fp == NULL) { error("Could not open file for reading\r\n"); }
00366     while(fgets (buf, 15, fp) != NULL){
00367         buf[strlen(buf)-1] = 0;
00368         }
00369     fclose(fp);
00370     k_th = (float)atof(buf);
00371     printf("k_th is loaded as %f. \r\n", k_th);
00372 
00373       // 5 -- b_th read
00374       fp = fopen("/sd/mydir/param/b_th.txt", "r");
00375       memset(buf, '\0', strlen(buf));
00376       if(fp == NULL) { error("Could not open file for reading\r\n"); }
00377       while(fgets (buf, 15, fp) != NULL){
00378           buf[strlen(buf)-1] = 0;
00379           }
00380       fclose(fp);
00381       b_th = (float)atof(buf);
00382       printf("b_th is loaded as %f. \r\n", b_th);
00383 
00384       // 6 -- v_th read
00385       fp = fopen("/sd/mydir/param/v_th.txt", "r");
00386       memset(buf, '\0', strlen(buf));
00387       if(fp == NULL) { error("Could not open file for reading\r\n"); }
00388       while(fgets (buf, 15, fp) != NULL){
00389           buf[strlen(buf)-1] = 0;
00390           }
00391       fclose(fp);
00392       v_th = (float)atof(buf);
00393       printf("v_th is loaded as %f. \r\n", v_th);
00394 
00395       // 7 -- flipMode read
00396       fp = fopen("/sd/mydir/param/flipMode.txt", "r");
00397       memset(buf, '\0', strlen(buf));
00398       if(fp == NULL) { error("Could not open file for reading\r\n"); }
00399       while(fgets (buf, 15, fp) != NULL){
00400           buf[strlen(buf)-1] = 0;
00401           }
00402       fclose(fp);
00403       flipMode = (float)atof(buf);
00404       printf("flipMode is loaded as %f. \r\n", flipMode);
00405 
00406       // 8 -- torqueCol_d read
00407       fp = fopen("/sd/mydir/param/torqueCol_d.txt", "r");
00408       memset(buf, '\0', strlen(buf));
00409       if(fp == NULL) { error("Could not open file for reading\r\n"); }
00410       while(fgets (buf, 15, fp) != NULL){
00411           buf[strlen(buf)-1] = 0;
00412           }
00413       fclose(fp);
00414       torqueCol_d = (float)atof(buf);
00415       printf("torqueCol_d is loaded as %f. \r\n", torqueCol_d);
00416 
00417       // 9 -- torqueFlight_d read
00418       fp = fopen("/sd/mydir/param/torqueFlight_d.txt", "r");
00419       memset(buf, '\0', strlen(buf));
00420       if(fp == NULL) { error("Could not open file for reading\r\n"); }
00421       while(fgets (buf, 15, fp) != NULL){
00422           buf[strlen(buf)-1] = 0;
00423           }
00424       fclose(fp);
00425       torqueFlight_d = (float)atof(buf);
00426       printf("torqueFlight_d is loaded as %f. \r\n", torqueFlight_d);
00427 
00428 }
00429 
00430 void saveParam(){ //to SD
00431   char buf[10];
00432 
00433   // 0 -- collisionDuration
00434   FILE *fp = fopen("/sd/mydir/param/collisionDuration.txt", "w");
00435   if(fp == NULL) {
00436       error("Could not open file for write\n");
00437   }
00438   memset(buf, '\0', strlen(buf));
00439   sprintf(buf, "%.3f", collisionDuration);
00440   fprintf(fp, buf);
00441   fprintf(fp, "\r\n");
00442   fclose(fp);
00443   printf("collisionDuration saved as %f.\r\n", collisionDuration);
00444 
00445     // 1 -- kp
00446     fp = fopen("/sd/mydir/param/kp.txt", "w");
00447     if(fp == NULL) {
00448         error("Could not open file for write\n");
00449     }
00450     memset(buf, '\0', strlen(buf));
00451     sprintf(buf, "%.3f", Kp);
00452     fprintf(fp, buf);
00453     fprintf(fp, "\r\n");
00454     fclose(fp);
00455     printf("Kp saved as %f.\r\n", Kp);
00456 
00457     // 2 -- kd
00458     fp = fopen("/sd/mydir/param/kd.txt", "w");
00459     if(fp == NULL) {
00460         error("Could not open file for write\n");
00461     }
00462     memset(buf, '\0', strlen(buf));
00463     sprintf(buf, "%.3f", Kd);
00464     fprintf(fp, buf);
00465     fprintf(fp, "\r\n");
00466     fclose(fp);
00467     printf("Kd saved as %f.\r\n", Kd);
00468 
00469     // 3 -- theta_d
00470     fp = fopen("/sd/mydir/param/theta_d.txt", "w");
00471     if(fp == NULL) {
00472         error("Could not open file for write\n");
00473     }
00474     memset(buf, '\0', strlen(buf));
00475     sprintf(buf, "%.3f", theta_d);
00476     fprintf(fp, buf);
00477     fprintf(fp, "\r\n");
00478     fclose(fp);
00479     printf("theta_d saved as %f.\r\n", theta_d);
00480 
00481     // 4 -- k_th
00482     fp = fopen("/sd/mydir/param/k_th.txt", "w");
00483     if(fp == NULL) {
00484         error("Could not open file for write\n");
00485     }
00486     memset(buf, '\0', strlen(buf));
00487     sprintf(buf, "%.3f", k_th);
00488     fprintf(fp, buf);
00489     fprintf(fp, "\r\n");
00490     fclose(fp);
00491     printf("k_th saved as %f.\r\n", k_th);
00492 
00493       // 5 -- b_th
00494       fp = fopen("/sd/mydir/param/b_th.txt", "w");
00495       if(fp == NULL) {
00496           error("Could not open file for write\n");
00497       }
00498       memset(buf, '\0', strlen(buf));
00499       sprintf(buf, "%.3f", b_th);
00500       fprintf(fp, buf);
00501       fprintf(fp, "\r\n");
00502       fclose(fp);
00503       printf("b_th saved as %f.\r\n", b_th);
00504 
00505       // 6 -- v_th
00506       fp = fopen("/sd/mydir/param/v_th.txt", "w");
00507       if(fp == NULL) {
00508           error("Could not open file for write\n");
00509       }
00510       memset(buf, '\0', strlen(buf));
00511       sprintf(buf, "%.3f", v_th);
00512       fprintf(fp, buf);
00513       fprintf(fp, "\r\n");
00514       fclose(fp);
00515       printf("v_th saved as %f.\r\n", v_th);
00516 
00517       // 7 -- flipMode
00518       fp = fopen("/sd/mydir/param/flipMode.txt", "w");
00519       if(fp == NULL) {
00520           error("Could not open file for write\n");
00521       }
00522       memset(buf, '\0', strlen(buf));
00523       sprintf(buf, "%.3f", flipMode);
00524       fprintf(fp, buf);
00525       fprintf(fp, "\r\n");
00526       fclose(fp);
00527       printf("flipMode saved as %f.\r\n", flipMode);
00528 
00529       // 8 -- torqueCol_d
00530       fp = fopen("/sd/mydir/param/torqueCol_d.txt", "w");
00531       if(fp == NULL) {
00532           error("Could not open file for write\n");
00533       }
00534       memset(buf, '\0', strlen(buf));
00535       sprintf(buf, "%.3f", torqueCol_d);
00536       fprintf(fp, buf);
00537       fprintf(fp, "\r\n");
00538       fclose(fp);
00539       printf("torqueCol_d saved as %f.\r\n", torqueCol_d);
00540 
00541       // 9 -- torqueFlight_d
00542       fp = fopen("/sd/mydir/param/torqueFlight_d.txt", "w");
00543       if(fp == NULL) {
00544           error("Could not open file for write\n");
00545       }
00546       memset(buf, '\0', strlen(buf));
00547       sprintf(buf, "%.3f", torqueFlight_d);
00548       fprintf(fp, buf);
00549       fprintf(fp, "\r\n");
00550       fclose(fp);
00551       printf("torqueFlight_d saved as %f.\r\n", torqueFlight_d);
00552 }
00553 
00554 char buffer_serial[20];
00555 void serialUpdateVal(){
00556 
00557       pc.scanf("%s", &buffer_serial);
00558       pc.printf("I received ");
00559       pc.printf(buffer_serial);
00560       pc.printf("\n");
00561 
00562       if(inputMode == 'n'){
00563         if(buffer_serial[0] == 'a'){
00564             pc.printf("input mode is set to collisionDuration, currently %f. Please enter value. \r\n", collisionDuration);
00565             inputMode = 'a';
00566          } else if(buffer_serial[0] == 'p'){
00567              pc.printf("input mode is set to Kp, currently %f. Please enter value. \r\n", Kp);
00568              inputMode = 'p';
00569           } else if(buffer_serial[0] == 'd'){
00570             pc.printf("input mode is set to Kd, currently %f. Please enter value. \r\n", Kd);
00571             inputMode = 'd';
00572          } else if(buffer_serial[0] == 't'){
00573             pc.printf("input mode is set to theta_d, currently %f. Please enter value. \r\n", theta_d);
00574             inputMode = 't';
00575          } else if(buffer_serial[0] == 'e'){ //k_th
00576              pc.printf("input mode is set to k_th, currently %f. Please enter value. \r\n", k_th);
00577              inputMode = 'e';
00578           } else if(buffer_serial[0] == 'b'){ //b_th
00579             pc.printf("input mode is set to b_th, currently %f. Please enter value. \r\n", b_th);
00580             inputMode = 'b';
00581          } else if(buffer_serial[0] == 'v'){ //v_th
00582             pc.printf("input mode is set to v_th, currently %f. Please enter value. \r\n", v_th);
00583             inputMode = 'v';
00584          } else if(buffer_serial[0] == 'f'){ //flipMode
00585              pc.printf("input mode is set to flipMode, currently %f. Please enter value. \r\n", flipMode);
00586              inputMode = 'f';
00587           } else if(buffer_serial[0] == 'g'){ //torqueCol_d
00588             pc.printf("input mode is set to torqueCol_d, currently %f. Please enter value. \r\n", torqueCol_d);
00589             inputMode = 'g';
00590          } else if(buffer_serial[0] == 'h'){ //torqueFlight_d
00591             pc.printf("input mode is set to torqueFlight_d, currently %f. Please enter value. \r\n", torqueFlight_d);
00592             inputMode = 'h';
00593          }
00594 
00595 
00596        } else if(inputMode == 'a'){
00597                 collisionDuration = (float)atof(buffer_serial);
00598                 inputMode = 'n';
00599                 pc.printf("collisionDuration is set to %f \r\n", collisionDuration);
00600                 saveParam();
00601         } else if(inputMode == 'p'){
00602                 Kp = (float)atof(buffer_serial);
00603                 inputMode = 'n';
00604                 pc.printf("Kp is set to %f \r\n", Kp);
00605                 saveParam();
00606         } else if(inputMode == 'd'){
00607                 Kd = (float)atof(buffer_serial);
00608                 inputMode = 'n';
00609                 pc.printf("Kd is set to %f \r\n", Kd);
00610                 saveParam();
00611         } else if(inputMode == 't'){
00612                 theta_d = (float)atof(buffer_serial);
00613                 inputMode = 'n';
00614                 pc.printf("theta_d is set to %f \r\n", theta_d);
00615                 saveParam();
00616         } else if(inputMode == 'e'){
00617                  k_th = (float)atof(buffer_serial);
00618                  inputMode = 'n';
00619                  pc.printf("k_th is set to %f \r\n", k_th);
00620                  saveParam();
00621          } else if(inputMode == 'b'){
00622                  b_th = (float)atof(buffer_serial);
00623                  inputMode = 'n';
00624                  pc.printf("b_th is set to %f \r\n", b_th);
00625                  saveParam();
00626          } else if(inputMode == 'v'){
00627                  v_th = (float)atof(buffer_serial);
00628                  inputMode = 'n';
00629                  pc.printf("v_th is set to %f \r\n", v_th);
00630                  saveParam();
00631          } else if(inputMode == 'f'){
00632                  flipMode = (float)atof(buffer_serial);
00633                  inputMode = 'n';
00634                  pc.printf("flipMode is set to %f \r\n", flipMode);
00635                  saveParam();
00636          }  else if(inputMode == 'g'){
00637                  torqueCol_d = (float)atof(buffer_serial);
00638                  inputMode = 'n';
00639                  pc.printf("torqueCol_d is set to %f \r\n", torqueCol_d);
00640                  saveParam();
00641          } else if(inputMode == 'h'){
00642                  torqueFlight_d = (float)atof(buffer_serial);
00643                  inputMode = 'n';
00644                  pc.printf("torqueFlight_d is set to %f \r\n", torqueFlight_d);
00645                  saveParam();
00646          }
00647 
00648 
00649 
00650 
00651         if(buffer_serial[0] == 'c'){
00652             pc.printf("collisionDuration: % f | Kp: %f | Kd: %f | theta_d: %f | k_th: %f | b_th: %f | v_th: %f | flipMode: %f | torqueCol_d: %f | torqueFlight_d: %f \r\n",
00653             collisionDuration, Kp, Kd, theta_d, k_th, b_th, v_th, flipMode, torqueCol_d, torqueFlight_d);
00654         }
00655 
00656 }
00657 
00658 /******************** LogData to SD card  *****************************/
00659 
00660 int trialTimeCount = 0;
00661 
00662 const int logValNum = 16;
00663 char logValName[logValNum][30];
00664 float logVal[logValNum][2500];
00665 int currentLogNum = 0;
00666 
00667 void updateTrialTime(){
00668     //trial time read
00669     FILE *fp_tr = fopen("/sd/mydir/trialtime.txt", "r");
00670 
00671     char Buffer_t[512];
00672 
00673     if(fp_tr == NULL) { error("Could not open file for reading\r\n"); }
00674 
00675     while(fgets (Buffer_t, 512, fp_tr) != NULL){
00676         Buffer_t[strlen(Buffer_t)-1] = 0;
00677         printf("String = \"%s\" \r\n", Buffer_t);
00678         }
00679     fclose(fp_tr);
00680 
00681     trialTimeCount = (int)atof(Buffer_t);
00682 
00683     printf("last trialTimeCount was %i. \n", trialTimeCount);
00684 
00685     trialTimeCount++; //count up trial time
00686     printf("current trialTimeCount updated to %i. \n", trialTimeCount);
00687 
00688     FILE *fp3 = fopen("/sd/mydir/trialtime.txt", "w");
00689     if(fp3 == NULL) {
00690         error("Could not open file for write\n");
00691     }
00692     char buf[10];
00693 
00694     sprintf(buf, "%d", trialTimeCount);
00695 
00696     fprintf(fp3, buf);
00697     fprintf(fp3, "\r\n");
00698     fclose(fp3);
00699     printf("trial time saved\n");
00700 }
00701 
00702 void logData(){ // log data time
00703 
00704 
00705     logVal[0][currentLogNum] = t.read();
00706     logVal[1][currentLogNum] = (int)bool_collision;
00707     logVal[2][currentLogNum] = theta;
00708     logVal[3][currentLogNum] = theta_dot;
00709     logVal[4][currentLogNum] = dT;
00710     logVal[5][currentLogNum] = i;
00711     logVal[6][currentLogNum] = outputVal;
00712     logVal[7][currentLogNum] = torque;
00713     logVal[8][currentLogNum] = kalmanAngle;
00714     logVal[9][currentLogNum] = accX;
00715     logVal[10][currentLogNum] = accY;
00716     logVal[11][currentLogNum] = accZ;
00717     logVal[12][currentLogNum] = gyroX;
00718     logVal[13][currentLogNum] = gyroY;
00719     logVal[14][currentLogNum] = gyroZ;
00720 
00721     //printf("logged data for %i. t.read() = %f \r\n", currentLogNum, t.read());
00722 
00723     currentLogNum++;
00724 }
00725 
00726 void saveData(){ // call when the while loop ends or the button pressed
00727    sprintf( logValName[0], "time");
00728    sprintf( logValName[1], "bool_collision");
00729    sprintf( logValName[2], "theta");
00730    sprintf( logValName[3], "theta_dot");
00731    sprintf( logValName[4], "dT");
00732    sprintf( logValName[5], "current");
00733    sprintf( logValName[6], "outputVal");
00734    sprintf( logValName[7], "torque");
00735    sprintf( logValName[8], "kalmanAngle");
00736    sprintf( logValName[9], "accX");
00737    sprintf( logValName[10], "accY");
00738    sprintf( logValName[11], "accZ");
00739    sprintf( logValName[12], "gyroX");
00740    sprintf( logValName[13], "gyroY");
00741    sprintf( logValName[14], "gyroZ");
00742 
00743 
00744     char filename[256];
00745     sprintf(filename, "/sd/mydir/log/flipper_logData_%i.txt",trialTimeCount);
00746 
00747     FILE *f_sv = fopen(filename, "w");
00748     if(f_sv == NULL) {
00749         error("Could not open file for write\n");
00750     }
00751 
00752     for(int i = 0; i < logValNum; i++){
00753         fprintf(f_sv, logValName[i]);
00754         fprintf(f_sv, ",");
00755     }
00756     fprintf(f_sv, "\r\n");
00757 
00758     for(int j = 0; j < currentLogNum; j++){
00759     for(int i = 0; i < logValNum; i++){
00760         //char buf_temp[10];
00761         //int a = 5;
00762 
00763         char cVal[8];
00764 
00765          sprintf(cVal,"%.3f", logVal[i][j]);
00766 
00767         //sprintf(buf_temp, "%f", logVal[i][j]);
00768 
00769         fprintf(f_sv, cVal);
00770         fprintf(f_sv, ",");
00771     }
00772     fprintf(f_sv, "\r\n");
00773     }
00774 
00775 
00776     fclose(f_sv);
00777 
00778     printf("Log Data file is saved as 'flipper_logData_%i.txt'.\n", trialTimeCount);
00779 }
00780 
00781 /*******************************************************************/
00782 
00783 /******************** FLIP STRATEGIES *****************************/
00784 
00785 float targetTheta = 0.0;
00786 float stiffnessStrategy(bool collision){
00787   // depends on global variables
00788   // theta_d: reference angle
00789   // theta: measured angle at joint B
00790   // theta_dot: measured angular velocity at joint B
00791   //if (collisionHappened<1){
00792   if (!collision){
00793     targetTheta = 0.0;
00794   }else{
00795     targetTheta = theta_d;
00796   }
00797 
00798   //printf("TARGET THETA %f\n",targetTheta);
00799 
00800 
00801   e = targetTheta-theta;
00802   /*torque = -k_th*(theta-theta_d)-b_th*theta_dot+v_th*theta_dot;
00803   if (torque<0){
00804     torque = max(-1.4, torque);
00805   }else{
00806     torque = min(1.4, torque);
00807   }
00808  // printf("torque %f\n", torque);
00809 
00810   //i_d = torque/k_b;
00811   //i = readCurrent();
00812   //e = i_d-i;
00813   //outputVal = r*i_d+Kp*(e)-k_b*theta_dot;*/
00814   //printf("%f\n",e);
00815   outputVal = k_th*e+b_th*(-theta_dot);
00816   //outputVal = Kp*e + Kd*(-theta_dot);
00817   return outputVal;
00818 }
00819 
00820 float torqueStrategy(bool collision){
00821   //uses global variables i,r
00822   //strategy begins at collisions
00823   if (collision){
00824     torque = torqueCol_d; //predefined value
00825   }else{
00826     if (hitTimes>0){
00827     torque = torqueFlight_d;
00828     }
00829     else{
00830     torque = 0;    
00831     }
00832   }
00833   // computation for output from torque
00834   // where these torques are discerned from
00835   // simulation profile
00836   i_d = torque/k_b;
00837   i = readCurrent();
00838   e = i_d-i;
00839   //printf("%f\n",outputVal);
00840   if (torque==0){
00841       outputVal=0;
00842   }
00843   else{
00844   outputVal = r*i_d+Kp*(e)-k_b*theta_dot;
00845   }
00846   return outputVal;
00847 }
00848 
00849 float stiffnessAndtorqueStrategy(bool collisionState){
00850   return stiffnessStrategy(collisionState)+torqueStrategy(collisionState);
00851 }
00852 
00853 float executeFlipStrategy(int flipmode, bool collisionState){
00854 
00855   // Mode 1: Stiffness only
00856   if (flipmode==1){
00857     return stiffnessStrategy(collisionState);
00858   }else{
00859     // Mode 2: Torque Profile only
00860     if (flipmode==2){
00861       return torqueStrategy(collisionState);
00862     }
00863     // Mode 3: Stiffness and Torque
00864     else{
00865       return stiffnessAndtorqueStrategy(collisionState);
00866     }
00867   }
00868 
00869 }
00870 
00871 
00872 /******************** EXECUTE MOTOR LOOP *****************************/
00873 
00874 float pikeAngle = 0.0;
00875 
00876 /** Core Motor Code Loop**/
00877 void executeMotorLoop(){
00878 
00879     printf("Entering Motor Loop\n");
00880     // initial setup
00881     t.reset();
00882     t.start();
00883     encoder.reset();
00884     setMotorDir('f'); //begin with motor driving forward
00885     motorPWM.write(0);
00886     lastTime=t.read();
00887     startTimeReference = t.read(); //set reference time for Kalman Angle
00888 
00889     /* Run experiment */
00890     while( t.read() < 10.0 && !debugModeOn) {
00891       //Update IMU and Kalman
00892       readIMU();
00893 
00894       //read present time and calculate dt
00895       presentTime = t.read();
00896       dT = presentTime - lastTime;
00897 
00898       // set last time to determine next time step
00899       lastTime = presentTime;
00900 
00901       // Perform control loop logic
00902       theta = getAngleFromPulses(encoder.getPulses());
00903       theta_dot = getAngleFromPulses(encoder.getVelocity());
00904 
00905       /* Checking collisions */
00906 
00907       // Determines whether new hit.
00908       if ((!bool_collision) && checkCollision(accX,dT) && (hitTimes<1)){
00909         printf("i am dumb\n");
00910         collisionTime = t.read();
00911         collisionTimeRemaining = collisionDuration;
00912         bool_collision = true;
00913         hitTimes = hitTimes + 1;
00914       }
00915       // If not a new hit, see if we are still in "hit state" from
00916       // collision duration parameter for actuation
00917       else{
00918         if ((bool_collision)&&(hitTimes<=1)){
00919           collisionTimeRemaining = (max((collisionDuration)-(t.read()-collisionTime),0.0));
00920           if (collisionTimeRemaining<=0.0){
00921             bool_collision = false;
00922           }
00923           }
00924           }
00925         //bool_collision = checkCollision(accX,dT);
00926 
00927 
00928       /* actuation strategy */
00929       // flip mode 1: stiffness only
00930       // flip mode 2: torque driving only
00931       // flip mode 3: torque driving with stiffness
00932 
00933 
00934       // Initial State, zero torque or other affects.
00935       torque = 0.0;
00936 
00937       /* Compute Actuation Force */
00938       outputVal = executeFlipStrategy(flipMode, bool_collision);
00939 
00940       /* Actuate Motors */
00941       if (outputVal<0){
00942     //negative difference, need to move motor backward to correct
00943     setMotorDir('f');
00944       }
00945       if (outputVal>0){
00946     //positive difference, need to move motor forward to correct
00947     setMotorDir('r');
00948       }
00949       motorPWM.write(toDutyCycle(abs(outputVal)));
00950       //printf("Serial: %f\n",outputVal);
00951 
00952       /* Log Data */
00953       logData();
00954       if(!saveDataButton){
00955     saveData();
00956     debugModeOn = true;
00957       }
00958 
00959     }
00960     // End of action cycle, turn motor "off"
00961     motorPWM.write(0);
00962 }
00963 
00964 
00965 
00966 
00967 /****************************** MAIN *************************************/
00968 
00969 int main()
00970 {
00971 
00972   printf("test!!\n");
00973   updateTrialTime();
00974   loadParam();
00975 
00976   /* setup imu and kalman filter for rotation */
00977   setupIMU();
00978   setupKalman();
00979 
00980   /* run core motor loop */
00981   executeMotorLoop();
00982 
00983   /* save trial data to sd card */
00984   saveData();
00985 
00986   printf("collisionDuration: % f | Kp: %f | Kd: %f | theta_d: %f | k_th: %f | b_th: %f | v_th: %f | flipMode: %f | torqueCol_d: %f | torqueFlight_d: %f \r\n",
00987   collisionDuration, Kp, Kd, theta_d, k_th, b_th, v_th, flipMode, torqueCol_d, torqueFlight_d);
00988   printf("Now you can set parameters by typing; 'a' - collisionDuration, 'p' - Kp, 'd' - Kd, 't' - theta_d, 'e' - k_th, 'b' - b_th, 'v' - v_th, 'f' - flipMode, 'g' - torqueCol_d, 'h' - torqueFlight_d.\n You can check the values with 'c'.");
00989 
00990   while(1){ // debugMode
00991     serialUpdateVal();
00992   }
00993 
00994 }