Pike Flippers / Mbed 2 deprecated Pike_the_Flipper_Main_Branch

Pike Bots for everyone! Uses a MAX32630 as the core. Incorporates Impedance control and SD card libaries for writing data. Reading from current sensor is not perfect due to limited ADC compared to datasheet, to fix in a future version.

Dependencies:   BMI160 QEI_pmw SDFileSystem USBDevice kalman max32630fthr mbed

Fork of Pike_the_Flipper_Main_Branch by Daniel Levine

main.cpp@4:227f0847a797, 2018-03-13 (annotated)

Committer:
DVLevine
Date:
Tue Mar 13 04:00:09 2018 +0000
Revision:
4:227f0847a797
Parent:
3:5696ac47658a 
Final Flipping Robot Pike. Writes to SD, has kalman integration. Not too shabby

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DVLevine 1:59124c69d0c3 1 #include <mbed.h>
DVLevine 1:59124c69d0c3 2 #include "max32630fthr.h"
DVLevine 1:59124c69d0c3 3 #include "bmi160.h"
DVLevine 1:59124c69d0c3 4 #include "I2C.h"
DVLevine 1:59124c69d0c3 5 #include "QEI.h"
DVLevine 1:59124c69d0c3 6 #include "kalman.c"
DVLevine 0:73a369b7b5b4 7 #include "SDFileSystem.h"
kennakagaki 2:983a818c6ddf 8 #include <string>
kennakagaki 2:983a818c6ddf 9 #include "USBSerial.h"
DVLevine 3:5696ac47658a 10
DVLevine 1:59124c69d0c3 11 //Defining PI
DVLevine 1:59124c69d0c3 12 #ifndef M_PI
DVLevine 1:59124c69d0c3 13 #define M_PI 3.1415
DVLevine 1:59124c69d0c3 14 #endif
DVLevine 1:59124c69d0c3 15
DVLevine 4:227f0847a797 16 float hitTimes = 0;
DVLevine 3:5696ac47658a 17 /********************** PIN DEFINITIONS ****************************/
DVLevine 1:59124c69d0c3 18 PwmOut motorPWM(P4_0); // Motor PWM output
DVLevine 1:59124c69d0c3 19 DigitalOut motorFwd(P5_6); // Motor forward enable
DVLevine 1:59124c69d0c3 20 DigitalOut motorRev(P5_5); // Motor backward enable
DVLevine 1:59124c69d0c3 21
DVLevine 1:59124c69d0c3 22 //Analog input
DVLevine 1:59124c69d0c3 23 AnalogIn ain(AIN_5);
DVLevine 1:59124c69d0c3 24 Timer t; // Timer to measure elapsed time of experiment
DVLevine 1:59124c69d0c3 25 QEI encoder(P5_3,P5_4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
DVLevine 1:59124c69d0c3 26
DVLevine 1:59124c69d0c3 27 DigitalOut led1(LED1, 1);
DVLevine 1:59124c69d0c3 28
kennakagaki 2:983a818c6ddf 29 // added by ken //
kennakagaki 2:983a818c6ddf 30 DigitalIn saveDataButton(P2_3);
kennakagaki 2:983a818c6ddf 31 SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // the pinout on the mbed Cool Components workshop board
kennakagaki 2:983a818c6ddf 32 Serial pc(USBTX, USBRX);
kennakagaki 2:983a818c6ddf 33 // added by ken - ends here //
kennakagaki 2:983a818c6ddf 34
DVLevine 3:5696ac47658a 35 /********************INSTANTIATING BOARD and SENSORS *************************/
DVLevine 3:5696ac47658a 36 //Board Initialization
DVLevine 1:59124c69d0c3 37 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
DVLevine 3:5696ac47658a 38
DVLevine 3:5696ac47658a 39 // IMU Initialization
DVLevine 1:59124c69d0c3 40 I2C i2cBus_acc(P5_7, P6_0);
DVLevine 3:5696ac47658a 41 BMI160_I2C* m_imu;
DVLevine 1:59124c69d0c3 42 BMI160::AccConfig accConfig;
DVLevine 1:59124c69d0c3 43 BMI160::GyroConfig gyroConfig;
DVLevine 1:59124c69d0c3 44 uint32_t failures = 0;
DVLevine 1:59124c69d0c3 45
DVLevine 3:5696ac47658a 46 /************** VALUES FOR STORING IMU and KALMAN ANGLE **********************/
DVLevine 3:5696ac47658a 47
DVLevine 3:5696ac47658a 48 // IMU Variables
DVLevine 1:59124c69d0c3 49 float imuTemperature;
DVLevine 1:59124c69d0c3 50 BMI160::SensorData accData;
DVLevine 1:59124c69d0c3 51 BMI160::SensorData gyroData;
DVLevine 1:59124c69d0c3 52 BMI160::SensorTime sensorTime;
DVLevine 1:59124c69d0c3 53 float accX = 0.0;
DVLevine 1:59124c69d0c3 54 float accY = 0.0;
DVLevine 1:59124c69d0c3 55 float accZ = 0.0;
DVLevine 1:59124c69d0c3 56 float gyroX = 0.0;
DVLevine 1:59124c69d0c3 57 float gyroY = 0.0;
DVLevine 1:59124c69d0c3 58 float gyroZ = 0.0;
DVLevine 1:59124c69d0c3 59
DVLevine 3:5696ac47658a 60 // Kalman Angle
DVLevine 3:5696ac47658a 61 float kalmanAngle = 0.0;
DVLevine 3:5696ac47658a 62
DVLevine 1:59124c69d0c3 63 // Collision Detection Variables - originally only for single collisions
DVLevine 1:59124c69d0c3 64 bool bool_collision = false; // collision state
DVLevine 1:59124c69d0c3 65 float collisionTime = 0.0; //time collision occurred
DVLevine 1:59124c69d0c3 66 float collisionTimeRemaining = 0.0; //variable to hold bool_collision for set time
DVLevine 4:227f0847a797 67 float collisionDuration = 0.2; // duration of collision (rough measured experimentally)
DVLevine 3:5696ac47658a 68
DVLevine 1:59124c69d0c3 69
DVLevine 1:59124c69d0c3 70 /****************** MOTOR CONTROL VARIABLES FOR STATES **********************/
DVLevine 1:59124c69d0c3 71 //For motor control
DVLevine 1:59124c69d0c3 72 //Variables for states
DVLevine 1:59124c69d0c3 73 float theta_d = 0.0;
DVLevine 3:5696ac47658a 74 float Kp = 3.0; // proportional gain
DVLevine 1:59124c69d0c3 75 float Kd = 1.2; //made up
DVLevine 1:59124c69d0c3 76 float Ki = 0.1; //made up
DVLevine 1:59124c69d0c3 77
DVLevine 1:59124c69d0c3 78 //Declare Parameters
DVLevine 1:59124c69d0c3 79 float e = 0.0; //error, theta_d-theta
DVLevine 1:59124c69d0c3 80 float theta = 0.0; //present theta
DVLevine 1:59124c69d0c3 81 float theta_dot = 0.0; //present angular velocity
DVLevine 1:59124c69d0c3 82 float val = 0.0; //
DVLevine 1:59124c69d0c3 83
DVLevine 1:59124c69d0c3 84 float lastTime = 0.0; //last time step
DVLevine 1:59124c69d0c3 85 float presentTime = 0.0; //current time step
DVLevine 4:227f0847a797 86 float dT = 0.045; //time difference
DVLevine 1:59124c69d0c3 87
DVLevine 3:5696ac47658a 88 /** Set gain parameters by log **/
DVLevine 1:59124c69d0c3 89 //Default Gains set to avoid problems
DVLevine 1:59124c69d0c3 90 float i_d = 0; //target current % changed by code
DVLevine 1:59124c69d0c3 91 float r = 3.2; // winding resistance;
DVLevine 1:59124c69d0c3 92 float k_b = 0.23; // back emf
DVLevine 3:5696ac47658a 93
DVLevine 1:59124c69d0c3 94 float k_th = 4; // proportional theta gain for torque
DVLevine 1:59124c69d0c3 95 float b_th = 0.2185; // proportional theta_dot gain for damping
DVLevine 3:5696ac47658a 96 float v_th = 0.0185; //viscous friction coefficient (from experiments)
DVLevine 1:59124c69d0c3 97
DVLevine 3:5696ac47658a 98 float torqueCol_d = 0.2; //torque target for collision
DVLevine 3:5696ac47658a 99 float torqueFlight_d = 0.0; //torque target for flight
kennakagaki 2:983a818c6ddf 100
DVLevine 1:59124c69d0c3 101 float i = 0.0; // present current
DVLevine 1:59124c69d0c3 102 float e_accum = 0.0;
DVLevine 1:59124c69d0c3 103
DVLevine 3:5696ac47658a 104 float torque = 0.0; //present torque
DVLevine 1:59124c69d0c3 105 float outputVal;//final compensation value (in rad) --> converted into dutycycle
DVLevine 1:59124c69d0c3 106
DVLevine 3:5696ac47658a 107 /********************** ACTUATION STRATEGY INDICATOR ***************************/
DVLevine 3:5696ac47658a 108 float flipMode = 1; //defaulty starts with stiffness strategy
kennakagaki 2:983a818c6ddf 109
DVLevine 3:5696ac47658a 110 /********************** DEBUG MODE VARIABLES ***********************************/
kennakagaki 2:983a818c6ddf 111 bool debugModeOn = false;
kennakagaki 2:983a818c6ddf 112
DVLevine 3:5696ac47658a 113
DVLevine 1:59124c69d0c3 114 /********************** FEEDBACK UTILITY FUNCTIONS ******************************/
DVLevine 1:59124c69d0c3 115 // A min function (as in min(a,b) or max(a,b)))
DVLevine 1:59124c69d0c3 116 float min(float a, float b){
DVLevine 1:59124c69d0c3 117 return a<b ? a : b;
DVLevine 1:59124c69d0c3 118 }
DVLevine 1:59124c69d0c3 119
DVLevine 1:59124c69d0c3 120 float max(float a, float b){
DVLevine 1:59124c69d0c3 121 return a>b ? a : b;
DVLevine 1:59124c69d0c3 122 }
DVLevine 1:59124c69d0c3 123 // for final angle to pwm duty cycle calc
DVLevine 1:59124c69d0c3 124 float toDutyCycle(float inputRad){
DVLevine 1:59124c69d0c3 125 float temp = abs(inputRad);
DVLevine 1:59124c69d0c3 126 if (temp>12){
DVLevine 1:59124c69d0c3 127 return 1.0;
DVLevine 1:59124c69d0c3 128 }else{
DVLevine 1:59124c69d0c3 129 return temp/12.0;
DVLevine 1:59124c69d0c3 130 }
DVLevine 1:59124c69d0c3 131 //abs(min(12.0,inputRad)/12.0);
DVLevine 1:59124c69d0c3 132 }
DVLevine 1:59124c69d0c3 133 // Calculates angles in radians from position
DVLevine 1:59124c69d0c3 134 float getAngleFromPulses(int input){
DVLevine 1:59124c69d0c3 135 return input*(2*M_PI)/1200.0;
DVLevine 1:59124c69d0c3 136 }
DVLevine 1:59124c69d0c3 137 // Set motor direction
DVLevine 1:59124c69d0c3 138 void setMotorDir(char dir){
DVLevine 1:59124c69d0c3 139 if (dir=='f'){
DVLevine 1:59124c69d0c3 140 motorFwd = 1;
DVLevine 1:59124c69d0c3 141 motorRev = 0;
DVLevine 1:59124c69d0c3 142 }else{
DVLevine 1:59124c69d0c3 143 motorFwd = 0;
DVLevine 1:59124c69d0c3 144 motorRev = 1;
DVLevine 1:59124c69d0c3 145 }
DVLevine 1:59124c69d0c3 146 }
DVLevine 1:59124c69d0c3 147
DVLevine 1:59124c69d0c3 148 // gets current
DVLevine 1:59124c69d0c3 149 float readCurrent(){
DVLevine 1:59124c69d0c3 150 return 36.7*ain.read()*(1.815)-18.3;
DVLevine 1:59124c69d0c3 151 // return 36.7*ain.read()*(1.815)-18.3;
DVLevine 1:59124c69d0c3 152 }
DVLevine 1:59124c69d0c3 153
DVLevine 1:59124c69d0c3 154 /********************* LED DISPLAY UTILITY FUNCTIONS *************************/
DVLevine 1:59124c69d0c3 155 PwmOut rLED(LED1);
DVLevine 1:59124c69d0c3 156 PwmOut gLED(LED2);
DVLevine 1:59124c69d0c3 157 PwmOut bLED(LED3);
DVLevine 1:59124c69d0c3 158
DVLevine 1:59124c69d0c3 159 void updateLEDS(float b1,float b2,float b3){
DVLevine 1:59124c69d0c3 160
DVLevine 1:59124c69d0c3 161 if(!bool_collision){
DVLevine 1:59124c69d0c3 162 rLED.write(max(1-b1/4,0.0));
DVLevine 1:59124c69d0c3 163 gLED.write(max(1-b2/4,0.0));
DVLevine 1:59124c69d0c3 164 bLED.write(max(1-b3/4,0.0));
DVLevine 1:59124c69d0c3 165 }else{
DVLevine 1:59124c69d0c3 166 rLED.write(0);
DVLevine 1:59124c69d0c3 167 gLED.write(0);
DVLevine 1:59124c69d0c3 168 bLED.write(1);
DVLevine 1:59124c69d0c3 169 }
DVLevine 1:59124c69d0c3 170 }
DVLevine 1:59124c69d0c3 171
DVLevine 1:59124c69d0c3 172 /********************* COLLISION UTILITY FUNCTIONS *************************/
DVLevine 1:59124c69d0c3 173 float last_xAcc = 0.0;
DVLevine 3:5696ac47658a 174 float jerk = 0.0;
DVLevine 4:227f0847a797 175 bool smashHappened = false;
DVLevine 4:227f0847a797 176
DVLevine 1:59124c69d0c3 177 bool checkCollision(float xAccel,float dT){
DVLevine 3:5696ac47658a 178 jerk = (xAccel-last_xAcc)/dT;
DVLevine 4:227f0847a797 179 printf("%f\n",(xAccel-last_xAcc));
DVLevine 4:227f0847a797 180 if ((xAccel-last_xAcc)<-1.5){//(jerk>100){
DVLevine 4:227f0847a797 181 return true;
DVLevine 4:227f0847a797 182 }
DVLevine 4:227f0847a797 183 else{
DVLevine 4:227f0847a797 184 if (collisionTimeRemaining >0.0){
DVLevine 4:227f0847a797 185 return true;
DVLevine 4:227f0847a797 186 }else{
DVLevine 3:5696ac47658a 187 return false;
DVLevine 4:227f0847a797 188 }
DVLevine 3:5696ac47658a 189 }
DVLevine 1:59124c69d0c3 190 }
DVLevine 1:59124c69d0c3 191
DVLevine 1:59124c69d0c3 192 //unused function for now
DVLevine 1:59124c69d0c3 193 float collisionActuation(float duration){
DVLevine 1:59124c69d0c3 194 //first decide start time
DVLevine 1:59124c69d0c3 195 //then actuate for a set duration
DVLevine 1:59124c69d0c3 196 return duration;
DVLevine 1:59124c69d0c3 197 }
DVLevine 1:59124c69d0c3 198
DVLevine 1:59124c69d0c3 199
DVLevine 1:59124c69d0c3 200
DVLevine 1:59124c69d0c3 201 /*****************************************************************************/
DVLevine 1:59124c69d0c3 202
DVLevine 1:59124c69d0c3 203 /******************** SETUP IMU *****************************/
DVLevine 1:59124c69d0c3 204 kalman filterRotation;
DVLevine 1:59124c69d0c3 205 double refAngle = 0.0;
DVLevine 1:59124c69d0c3 206 double qGyro = 0.0;
DVLevine 1:59124c69d0c3 207 double qAngle = 0.0;
DVLevine 1:59124c69d0c3 208
DVLevine 1:59124c69d0c3 209 void setupKalman(){
DVLevine 1:59124c69d0c3 210 kalman_init(&filterRotation, R_matrix,Q_Gyro_matrix,Q_Accel_matrix);
DVLevine 1:59124c69d0c3 211 }
DVLevine 1:59124c69d0c3 212
DVLevine 1:59124c69d0c3 213
DVLevine 1:59124c69d0c3 214 void setupIMU()
DVLevine 1:59124c69d0c3 215 {
DVLevine 1:59124c69d0c3 216 i2cBus_acc.frequency(400000);
DVLevine 1:59124c69d0c3 217 m_imu = new BMI160_I2C(i2cBus_acc, BMI160_I2C::I2C_ADRS_SDO_LO);
DVLevine 1:59124c69d0c3 218 printf("\033[H"); //home
DVLevine 1:59124c69d0c3 219 printf("\033[0J"); //erase from cursor to end of screen
DVLevine 1:59124c69d0c3 220 if(m_imu->setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
DVLevine 1:59124c69d0c3 221 printf("Failed to set gyroscope power mode\n");
DVLevine 1:59124c69d0c3 222 failures++;
DVLevine 1:59124c69d0c3 223 }
DVLevine 1:59124c69d0c3 224 wait_ms(100);
DVLevine 1:59124c69d0c3 225
DVLevine 1:59124c69d0c3 226 if(m_imu->setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
DVLevine 1:59124c69d0c3 227 printf("Failed to set accelerometer power mode\n");
DVLevine 1:59124c69d0c3 228 failures++;
DVLevine 1:59124c69d0c3 229 }
DVLevine 1:59124c69d0c3 230 wait_ms(100);
DVLevine 1:59124c69d0c3 231
DVLevine 1:59124c69d0c3 232
DVLevine 1:59124c69d0c3 233 //example of setting user defined configuration
DVLevine 1:59124c69d0c3 234 accConfig.range = BMI160::SENS_4G;
DVLevine 1:59124c69d0c3 235 accConfig.us = BMI160::ACC_US_OFF;
DVLevine 1:59124c69d0c3 236 accConfig.bwp = BMI160::ACC_BWP_2;
DVLevine 1:59124c69d0c3 237 accConfig.odr = BMI160::ACC_ODR_12; //reads at 160 kHz
DVLevine 1:59124c69d0c3 238
DVLevine 1:59124c69d0c3 239 if(m_imu->getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) {
DVLevine 1:59124c69d0c3 240 printf("ACC Range = %d\n", accConfig.range);
DVLevine 1:59124c69d0c3 241 printf("ACC UnderSampling = %d\n", accConfig.us);
DVLevine 1:59124c69d0c3 242 printf("ACC BandWidthParam = %d\n", accConfig.bwp);
DVLevine 1:59124c69d0c3 243 printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
DVLevine 1:59124c69d0c3 244 } else {
DVLevine 1:59124c69d0c3 245 printf("Failed to get accelerometer configuration\n");
DVLevine 1:59124c69d0c3 246 failures++;
DVLevine 1:59124c69d0c3 247 }
DVLevine 1:59124c69d0c3 248
DVLevine 1:59124c69d0c3 249 if(m_imu->getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) {
DVLevine 1:59124c69d0c3 250 printf("GYRO Range = %d\n", gyroConfig.range);
DVLevine 1:59124c69d0c3 251 printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
DVLevine 1:59124c69d0c3 252 printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
DVLevine 1:59124c69d0c3 253 } else {
DVLevine 1:59124c69d0c3 254 printf("Failed to get gyroscope configuration\n");
DVLevine 1:59124c69d0c3 255 failures++;
DVLevine 0:73a369b7b5b4 256 }
DVLevine 1:59124c69d0c3 257
DVLevine 1:59124c69d0c3 258 wait(1.0);
DVLevine 1:59124c69d0c3 259
DVLevine 1:59124c69d0c3 260 m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
DVLevine 1:59124c69d0c3 261 m_imu->getTemperature(&imuTemperature);
DVLevine 1:59124c69d0c3 262
DVLevine 1:59124c69d0c3 263 accX = accData.xAxis.scaled;
DVLevine 1:59124c69d0c3 264 accY = accData.yAxis.scaled;
DVLevine 1:59124c69d0c3 265 accZ = accData.zAxis.scaled;
DVLevine 1:59124c69d0c3 266
DVLevine 1:59124c69d0c3 267 printf("ACC xAxis = %s%4.3f\n", "\033[K", accX);
DVLevine 1:59124c69d0c3 268 printf("ACC xAxis = %s%4.3f\n", "\033[K", accY);
DVLevine 1:59124c69d0c3 269 printf("ACC xAxis = %s%4.3f\n", "\033[K", accZ);
DVLevine 1:59124c69d0c3 270
DVLevine 1:59124c69d0c3 271
DVLevine 1:59124c69d0c3 272 updateLEDS(accX,accY,accZ);
DVLevine 1:59124c69d0c3 273 }
DVLevine 1:59124c69d0c3 274
DVLevine 1:59124c69d0c3 275 /******************** READ IMU *****************************/
DVLevine 1:59124c69d0c3 276
DVLevine 1:59124c69d0c3 277 // ▯ x is up, y is left, z is out of the board (towards me). BLE module is on top.
DVLevine 1:59124c69d0c3 278 // R i = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
DVLevine 1:59124c69d0c3 279 void getKalmanPrediction(double dT, float gyroReading, float accReading, float R){
DVLevine 1:59124c69d0c3 280 kalman_predict(&filterRotation, gyroReading, dT);
DVLevine 1:59124c69d0c3 281 kalman_update(&filterRotation, acos(accReading/R));
DVLevine 1:59124c69d0c3 282 }
DVLevine 1:59124c69d0c3 283
DVLevine 1:59124c69d0c3 284
DVLevine 1:59124c69d0c3 285 float lastKalTime = 0.0;
DVLevine 1:59124c69d0c3 286 float R = 0.0;
DVLevine 3:5696ac47658a 287 float startTimeReference = 0.0;
DVLevine 1:59124c69d0c3 288
DVLevine 1:59124c69d0c3 289 void readIMU()
DVLevine 1:59124c69d0c3 290 {
DVLevine 1:59124c69d0c3 291 m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
DVLevine 1:59124c69d0c3 292 m_imu->getTemperature(&imuTemperature);
DVLevine 1:59124c69d0c3 293
DVLevine 1:59124c69d0c3 294 accX = accData.xAxis.scaled;
DVLevine 1:59124c69d0c3 295 accY = accData.yAxis.scaled;
DVLevine 1:59124c69d0c3 296 accZ = accData.zAxis.scaled;
DVLevine 1:59124c69d0c3 297
DVLevine 1:59124c69d0c3 298 gyroX = gyroData.xAxis.scaled;
DVLevine 1:59124c69d0c3 299 gyroY = gyroData.yAxis.scaled;
DVLevine 1:59124c69d0c3 300 gyroZ = gyroData.zAxis.scaled;
DVLevine 1:59124c69d0c3 301
DVLevine 3:5696ac47658a 302 R = sqrt(std::pow(accX, 2) + std::pow(accY, 2) + std::pow(accZ, 2));
DVLevine 3:5696ac47658a 303 getKalmanPrediction(t.read()-lastKalTime, gyroX, accX, R);
DVLevine 3:5696ac47658a 304 //printf("%f\n",kalman_get_angle(&filterRotation));
DVLevine 3:5696ac47658a 305 kalmanAngle = kalman_get_angle(&filterRotation);
DVLevine 1:59124c69d0c3 306
DVLevine 3:5696ac47658a 307 lastKalTime=t.read();
DVLevine 1:59124c69d0c3 308
DVLevine 1:59124c69d0c3 309 updateLEDS(accX,accY,accZ);
DVLevine 1:59124c69d0c3 310 }
kennakagaki 2:983a818c6ddf 311
DVLevine 3:5696ac47658a 312 /*********** Load and Update and Save Parameters ***************/
kennakagaki 2:983a818c6ddf 313 char inputMode = 'n';
kennakagaki 2:983a818c6ddf 314
kennakagaki 2:983a818c6ddf 315 void loadParam(){ //from SD
DVLevine 3:5696ac47658a 316 char buf[15];
DVLevine 3:5696ac47658a 317
DVLevine 3:5696ac47658a 318 // 0 -- collisionDuration read
DVLevine 3:5696ac47658a 319 FILE *fp = fopen("/sd/mydir/param/collisionDuration.txt", "r");
DVLevine 3:5696ac47658a 320 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 321 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 322 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 323 buf[strlen(buf)-1] = 0;
DVLevine 3:5696ac47658a 324 }
DVLevine 3:5696ac47658a 325 fclose(fp);
DVLevine 3:5696ac47658a 326 collisionDuration = (float)atof(buf);
DVLevine 3:5696ac47658a 327 printf("collisionDuration is loaded as %f. \r\n", collisionDuration);
DVLevine 3:5696ac47658a 328
DVLevine 3:5696ac47658a 329 // 1 -- kp read
DVLevine 3:5696ac47658a 330 fp = fopen("/sd/mydir/param/kp.txt", "r");
DVLevine 3:5696ac47658a 331 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 332 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 333 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 334 buf[strlen(buf)-1] = 0;
kennakagaki 2:983a818c6ddf 335 }
DVLevine 3:5696ac47658a 336 fclose(fp);
DVLevine 3:5696ac47658a 337 Kp = (float)atof(buf);
kennakagaki 2:983a818c6ddf 338 printf("Kp is loaded as %f. \r\n", Kp);
DVLevine 3:5696ac47658a 339
DVLevine 3:5696ac47658a 340 // 2 -- kd read
DVLevine 3:5696ac47658a 341 fp = fopen("/sd/mydir/param/kd.txt", "r");
DVLevine 3:5696ac47658a 342 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 343 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 344 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 345 buf[strlen(buf)-1] = 0;
kennakagaki 2:983a818c6ddf 346 }
DVLevine 3:5696ac47658a 347 fclose(fp);
DVLevine 3:5696ac47658a 348 Kd = (float)atof(buf);
kennakagaki 2:983a818c6ddf 349 printf("Kd is loaded as %f. \r\n", Kd);
DVLevine 3:5696ac47658a 350
DVLevine 3:5696ac47658a 351 // 3 -- theta_d read
DVLevine 3:5696ac47658a 352 fp = fopen("/sd/mydir/param/theta_d.txt", "r");
DVLevine 3:5696ac47658a 353 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 354 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 355 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 356 buf[strlen(buf)-1] = 0;
DVLevine 3:5696ac47658a 357 }
DVLevine 3:5696ac47658a 358 fclose(fp);
DVLevine 3:5696ac47658a 359 theta_d = (float)atof(buf);
DVLevine 3:5696ac47658a 360 printf("theta_d is loaded as %f. \r\n", theta_d);
DVLevine 3:5696ac47658a 361
DVLevine 3:5696ac47658a 362 // 4 -- k_th read
DVLevine 3:5696ac47658a 363 fp = fopen("/sd/mydir/param/k_th.txt", "r");
DVLevine 3:5696ac47658a 364 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 365 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 366 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 367 buf[strlen(buf)-1] = 0;
kennakagaki 2:983a818c6ddf 368 }
DVLevine 3:5696ac47658a 369 fclose(fp);
DVLevine 3:5696ac47658a 370 k_th = (float)atof(buf);
DVLevine 3:5696ac47658a 371 printf("k_th is loaded as %f. \r\n", k_th);
DVLevine 3:5696ac47658a 372
DVLevine 3:5696ac47658a 373 // 5 -- b_th read
DVLevine 3:5696ac47658a 374 fp = fopen("/sd/mydir/param/b_th.txt", "r");
DVLevine 3:5696ac47658a 375 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 376 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 377 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 378 buf[strlen(buf)-1] = 0;
DVLevine 3:5696ac47658a 379 }
DVLevine 3:5696ac47658a 380 fclose(fp);
DVLevine 3:5696ac47658a 381 b_th = (float)atof(buf);
DVLevine 3:5696ac47658a 382 printf("b_th is loaded as %f. \r\n", b_th);
DVLevine 3:5696ac47658a 383
DVLevine 3:5696ac47658a 384 // 6 -- v_th read
DVLevine 3:5696ac47658a 385 fp = fopen("/sd/mydir/param/v_th.txt", "r");
DVLevine 3:5696ac47658a 386 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 387 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 388 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 389 buf[strlen(buf)-1] = 0;
DVLevine 3:5696ac47658a 390 }
DVLevine 3:5696ac47658a 391 fclose(fp);
DVLevine 3:5696ac47658a 392 v_th = (float)atof(buf);
DVLevine 3:5696ac47658a 393 printf("v_th is loaded as %f. \r\n", v_th);
DVLevine 3:5696ac47658a 394
DVLevine 3:5696ac47658a 395 // 7 -- flipMode read
DVLevine 3:5696ac47658a 396 fp = fopen("/sd/mydir/param/flipMode.txt", "r");
DVLevine 3:5696ac47658a 397 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 398 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 399 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 400 buf[strlen(buf)-1] = 0;
DVLevine 3:5696ac47658a 401 }
DVLevine 3:5696ac47658a 402 fclose(fp);
DVLevine 3:5696ac47658a 403 flipMode = (float)atof(buf);
DVLevine 3:5696ac47658a 404 printf("flipMode is loaded as %f. \r\n", flipMode);
DVLevine 3:5696ac47658a 405
DVLevine 3:5696ac47658a 406 // 8 -- torqueCol_d read
DVLevine 3:5696ac47658a 407 fp = fopen("/sd/mydir/param/torqueCol_d.txt", "r");
DVLevine 3:5696ac47658a 408 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 409 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 410 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 411 buf[strlen(buf)-1] = 0;
DVLevine 3:5696ac47658a 412 }
DVLevine 3:5696ac47658a 413 fclose(fp);
DVLevine 3:5696ac47658a 414 torqueCol_d = (float)atof(buf);
DVLevine 3:5696ac47658a 415 printf("torqueCol_d is loaded as %f. \r\n", torqueCol_d);
DVLevine 3:5696ac47658a 416
DVLevine 3:5696ac47658a 417 // 9 -- torqueFlight_d read
DVLevine 3:5696ac47658a 418 fp = fopen("/sd/mydir/param/torqueFlight_d.txt", "r");
DVLevine 3:5696ac47658a 419 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 420 if(fp == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 421 while(fgets (buf, 15, fp) != NULL){
DVLevine 3:5696ac47658a 422 buf[strlen(buf)-1] = 0;
DVLevine 3:5696ac47658a 423 }
DVLevine 3:5696ac47658a 424 fclose(fp);
DVLevine 3:5696ac47658a 425 torqueFlight_d = (float)atof(buf);
DVLevine 3:5696ac47658a 426 printf("torqueFlight_d is loaded as %f. \r\n", torqueFlight_d);
DVLevine 3:5696ac47658a 427
kennakagaki 2:983a818c6ddf 428 }
kennakagaki 2:983a818c6ddf 429
kennakagaki 2:983a818c6ddf 430 void saveParam(){ //to SD
DVLevine 3:5696ac47658a 431 char buf[10];
kennakagaki 2:983a818c6ddf 432
DVLevine 3:5696ac47658a 433 // 0 -- collisionDuration
DVLevine 3:5696ac47658a 434 FILE *fp = fopen("/sd/mydir/param/collisionDuration.txt", "w");
DVLevine 3:5696ac47658a 435 if(fp == NULL) {
DVLevine 3:5696ac47658a 436 error("Could not open file for write\n");
DVLevine 3:5696ac47658a 437 }
DVLevine 3:5696ac47658a 438 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 439 sprintf(buf, "%.3f", collisionDuration);
DVLevine 3:5696ac47658a 440 fprintf(fp, buf);
DVLevine 3:5696ac47658a 441 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 442 fclose(fp);
DVLevine 3:5696ac47658a 443 printf("collisionDuration saved as %f.\r\n", collisionDuration);
DVLevine 3:5696ac47658a 444
DVLevine 3:5696ac47658a 445 // 1 -- kp
DVLevine 3:5696ac47658a 446 fp = fopen("/sd/mydir/param/kp.txt", "w");
DVLevine 3:5696ac47658a 447 if(fp == NULL) {
kennakagaki 2:983a818c6ddf 448 error("Could not open file for write\n");
kennakagaki 2:983a818c6ddf 449 }
DVLevine 3:5696ac47658a 450 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 451 sprintf(buf, "%.3f", Kp);
DVLevine 3:5696ac47658a 452 fprintf(fp, buf);
DVLevine 3:5696ac47658a 453 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 454 fclose(fp);
kennakagaki 2:983a818c6ddf 455 printf("Kp saved as %f.\r\n", Kp);
DVLevine 3:5696ac47658a 456
DVLevine 3:5696ac47658a 457 // 2 -- kd
DVLevine 3:5696ac47658a 458 fp = fopen("/sd/mydir/param/kd.txt", "w");
DVLevine 3:5696ac47658a 459 if(fp == NULL) {
DVLevine 3:5696ac47658a 460 error("Could not open file for write\n");
DVLevine 3:5696ac47658a 461 }
DVLevine 3:5696ac47658a 462 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 463 sprintf(buf, "%.3f", Kd);
DVLevine 3:5696ac47658a 464 fprintf(fp, buf);
DVLevine 3:5696ac47658a 465 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 466 fclose(fp);
DVLevine 3:5696ac47658a 467 printf("Kd saved as %f.\r\n", Kd);
DVLevine 3:5696ac47658a 468
DVLevine 3:5696ac47658a 469 // 3 -- theta_d
DVLevine 3:5696ac47658a 470 fp = fopen("/sd/mydir/param/theta_d.txt", "w");
DVLevine 3:5696ac47658a 471 if(fp == NULL) {
DVLevine 3:5696ac47658a 472 error("Could not open file for write\n");
DVLevine 3:5696ac47658a 473 }
DVLevine 3:5696ac47658a 474 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 475 sprintf(buf, "%.3f", theta_d);
DVLevine 3:5696ac47658a 476 fprintf(fp, buf);
DVLevine 3:5696ac47658a 477 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 478 fclose(fp);
DVLevine 3:5696ac47658a 479 printf("theta_d saved as %f.\r\n", theta_d);
DVLevine 3:5696ac47658a 480
DVLevine 3:5696ac47658a 481 // 4 -- k_th
DVLevine 3:5696ac47658a 482 fp = fopen("/sd/mydir/param/k_th.txt", "w");
DVLevine 3:5696ac47658a 483 if(fp == NULL) {
kennakagaki 2:983a818c6ddf 484 error("Could not open file for write\n");
kennakagaki 2:983a818c6ddf 485 }
DVLevine 3:5696ac47658a 486 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 487 sprintf(buf, "%.3f", k_th);
DVLevine 3:5696ac47658a 488 fprintf(fp, buf);
DVLevine 3:5696ac47658a 489 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 490 fclose(fp);
DVLevine 3:5696ac47658a 491 printf("k_th saved as %f.\r\n", k_th);
DVLevine 3:5696ac47658a 492
DVLevine 3:5696ac47658a 493 // 5 -- b_th
DVLevine 3:5696ac47658a 494 fp = fopen("/sd/mydir/param/b_th.txt", "w");
DVLevine 3:5696ac47658a 495 if(fp == NULL) {
DVLevine 3:5696ac47658a 496 error("Could not open file for write\n");
DVLevine 3:5696ac47658a 497 }
DVLevine 3:5696ac47658a 498 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 499 sprintf(buf, "%.3f", b_th);
DVLevine 3:5696ac47658a 500 fprintf(fp, buf);
DVLevine 3:5696ac47658a 501 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 502 fclose(fp);
DVLevine 3:5696ac47658a 503 printf("b_th saved as %f.\r\n", b_th);
DVLevine 3:5696ac47658a 504
DVLevine 3:5696ac47658a 505 // 6 -- v_th
DVLevine 3:5696ac47658a 506 fp = fopen("/sd/mydir/param/v_th.txt", "w");
DVLevine 3:5696ac47658a 507 if(fp == NULL) {
DVLevine 3:5696ac47658a 508 error("Could not open file for write\n");
DVLevine 3:5696ac47658a 509 }
DVLevine 3:5696ac47658a 510 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 511 sprintf(buf, "%.3f", v_th);
DVLevine 3:5696ac47658a 512 fprintf(fp, buf);
DVLevine 3:5696ac47658a 513 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 514 fclose(fp);
DVLevine 3:5696ac47658a 515 printf("v_th saved as %f.\r\n", v_th);
DVLevine 3:5696ac47658a 516
DVLevine 3:5696ac47658a 517 // 7 -- flipMode
DVLevine 3:5696ac47658a 518 fp = fopen("/sd/mydir/param/flipMode.txt", "w");
DVLevine 3:5696ac47658a 519 if(fp == NULL) {
DVLevine 3:5696ac47658a 520 error("Could not open file for write\n");
DVLevine 3:5696ac47658a 521 }
DVLevine 3:5696ac47658a 522 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 523 sprintf(buf, "%.3f", flipMode);
DVLevine 3:5696ac47658a 524 fprintf(fp, buf);
DVLevine 3:5696ac47658a 525 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 526 fclose(fp);
DVLevine 3:5696ac47658a 527 printf("flipMode saved as %f.\r\n", flipMode);
DVLevine 3:5696ac47658a 528
DVLevine 3:5696ac47658a 529 // 8 -- torqueCol_d
DVLevine 3:5696ac47658a 530 fp = fopen("/sd/mydir/param/torqueCol_d.txt", "w");
DVLevine 3:5696ac47658a 531 if(fp == NULL) {
DVLevine 3:5696ac47658a 532 error("Could not open file for write\n");
DVLevine 3:5696ac47658a 533 }
DVLevine 3:5696ac47658a 534 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 535 sprintf(buf, "%.3f", torqueCol_d);
DVLevine 3:5696ac47658a 536 fprintf(fp, buf);
DVLevine 3:5696ac47658a 537 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 538 fclose(fp);
DVLevine 3:5696ac47658a 539 printf("torqueCol_d saved as %f.\r\n", torqueCol_d);
DVLevine 3:5696ac47658a 540
DVLevine 3:5696ac47658a 541 // 9 -- torqueFlight_d
DVLevine 3:5696ac47658a 542 fp = fopen("/sd/mydir/param/torqueFlight_d.txt", "w");
DVLevine 3:5696ac47658a 543 if(fp == NULL) {
DVLevine 3:5696ac47658a 544 error("Could not open file for write\n");
DVLevine 3:5696ac47658a 545 }
DVLevine 3:5696ac47658a 546 memset(buf, '\0', strlen(buf));
DVLevine 3:5696ac47658a 547 sprintf(buf, "%.3f", torqueFlight_d);
DVLevine 3:5696ac47658a 548 fprintf(fp, buf);
DVLevine 3:5696ac47658a 549 fprintf(fp, "\r\n");
DVLevine 3:5696ac47658a 550 fclose(fp);
DVLevine 3:5696ac47658a 551 printf("torqueFlight_d saved as %f.\r\n", torqueFlight_d);
kennakagaki 2:983a818c6ddf 552 }
kennakagaki 2:983a818c6ddf 553
kennakagaki 2:983a818c6ddf 554 char buffer_serial[20];
kennakagaki 2:983a818c6ddf 555 void serialUpdateVal(){
DVLevine 3:5696ac47658a 556
kennakagaki 2:983a818c6ddf 557 pc.scanf("%s", &buffer_serial);
kennakagaki 2:983a818c6ddf 558 pc.printf("I received ");
kennakagaki 2:983a818c6ddf 559 pc.printf(buffer_serial);
kennakagaki 2:983a818c6ddf 560 pc.printf("\n");
DVLevine 3:5696ac47658a 561
kennakagaki 2:983a818c6ddf 562 if(inputMode == 'n'){
DVLevine 3:5696ac47658a 563 if(buffer_serial[0] == 'a'){
DVLevine 3:5696ac47658a 564 pc.printf("input mode is set to collisionDuration, currently %f. Please enter value. \r\n", collisionDuration);
DVLevine 3:5696ac47658a 565 inputMode = 'a';
DVLevine 3:5696ac47658a 566 } else if(buffer_serial[0] == 'p'){
DVLevine 3:5696ac47658a 567 pc.printf("input mode is set to Kp, currently %f. Please enter value. \r\n", Kp);
DVLevine 3:5696ac47658a 568 inputMode = 'p';
DVLevine 3:5696ac47658a 569 } else if(buffer_serial[0] == 'd'){
DVLevine 3:5696ac47658a 570 pc.printf("input mode is set to Kd, currently %f. Please enter value. \r\n", Kd);
kennakagaki 2:983a818c6ddf 571 inputMode = 'd';
DVLevine 3:5696ac47658a 572 } else if(buffer_serial[0] == 't'){
DVLevine 3:5696ac47658a 573 pc.printf("input mode is set to theta_d, currently %f. Please enter value. \r\n", theta_d);
kennakagaki 2:983a818c6ddf 574 inputMode = 't';
DVLevine 3:5696ac47658a 575 } else if(buffer_serial[0] == 'e'){ //k_th
DVLevine 3:5696ac47658a 576 pc.printf("input mode is set to k_th, currently %f. Please enter value. \r\n", k_th);
DVLevine 3:5696ac47658a 577 inputMode = 'e';
DVLevine 3:5696ac47658a 578 } else if(buffer_serial[0] == 'b'){ //b_th
DVLevine 3:5696ac47658a 579 pc.printf("input mode is set to b_th, currently %f. Please enter value. \r\n", b_th);
DVLevine 3:5696ac47658a 580 inputMode = 'b';
DVLevine 3:5696ac47658a 581 } else if(buffer_serial[0] == 'v'){ //v_th
DVLevine 3:5696ac47658a 582 pc.printf("input mode is set to v_th, currently %f. Please enter value. \r\n", v_th);
DVLevine 3:5696ac47658a 583 inputMode = 'v';
DVLevine 3:5696ac47658a 584 } else if(buffer_serial[0] == 'f'){ //flipMode
DVLevine 3:5696ac47658a 585 pc.printf("input mode is set to flipMode, currently %f. Please enter value. \r\n", flipMode);
DVLevine 3:5696ac47658a 586 inputMode = 'f';
DVLevine 3:5696ac47658a 587 } else if(buffer_serial[0] == 'g'){ //torqueCol_d
DVLevine 3:5696ac47658a 588 pc.printf("input mode is set to torqueCol_d, currently %f. Please enter value. \r\n", torqueCol_d);
DVLevine 3:5696ac47658a 589 inputMode = 'g';
DVLevine 3:5696ac47658a 590 } else if(buffer_serial[0] == 'h'){ //torqueFlight_d
DVLevine 3:5696ac47658a 591 pc.printf("input mode is set to torqueFlight_d, currently %f. Please enter value. \r\n", torqueFlight_d);
DVLevine 3:5696ac47658a 592 inputMode = 'h';
DVLevine 3:5696ac47658a 593 }
DVLevine 3:5696ac47658a 594
DVLevine 3:5696ac47658a 595
DVLevine 3:5696ac47658a 596 } else if(inputMode == 'a'){
DVLevine 3:5696ac47658a 597 collisionDuration = (float)atof(buffer_serial);
DVLevine 3:5696ac47658a 598 inputMode = 'n';
DVLevine 3:5696ac47658a 599 pc.printf("collisionDuration is set to %f \r\n", collisionDuration);
DVLevine 3:5696ac47658a 600 saveParam();
kennakagaki 2:983a818c6ddf 601 } else if(inputMode == 'p'){
kennakagaki 2:983a818c6ddf 602 Kp = (float)atof(buffer_serial);
kennakagaki 2:983a818c6ddf 603 inputMode = 'n';
DVLevine 3:5696ac47658a 604 pc.printf("Kp is set to %f \r\n", Kp);
kennakagaki 2:983a818c6ddf 605 saveParam();
kennakagaki 2:983a818c6ddf 606 } else if(inputMode == 'd'){
kennakagaki 2:983a818c6ddf 607 Kd = (float)atof(buffer_serial);
kennakagaki 2:983a818c6ddf 608 inputMode = 'n';
DVLevine 3:5696ac47658a 609 pc.printf("Kd is set to %f \r\n", Kd);
kennakagaki 2:983a818c6ddf 610 saveParam();
kennakagaki 2:983a818c6ddf 611 } else if(inputMode == 't'){
kennakagaki 2:983a818c6ddf 612 theta_d = (float)atof(buffer_serial);
kennakagaki 2:983a818c6ddf 613 inputMode = 'n';
DVLevine 3:5696ac47658a 614 pc.printf("theta_d is set to %f \r\n", theta_d);
kennakagaki 2:983a818c6ddf 615 saveParam();
DVLevine 3:5696ac47658a 616 } else if(inputMode == 'e'){
DVLevine 3:5696ac47658a 617 k_th = (float)atof(buffer_serial);
DVLevine 3:5696ac47658a 618 inputMode = 'n';
DVLevine 3:5696ac47658a 619 pc.printf("k_th is set to %f \r\n", k_th);
DVLevine 3:5696ac47658a 620 saveParam();
DVLevine 3:5696ac47658a 621 } else if(inputMode == 'b'){
DVLevine 3:5696ac47658a 622 b_th = (float)atof(buffer_serial);
DVLevine 3:5696ac47658a 623 inputMode = 'n';
DVLevine 3:5696ac47658a 624 pc.printf("b_th is set to %f \r\n", b_th);
DVLevine 3:5696ac47658a 625 saveParam();
DVLevine 3:5696ac47658a 626 } else if(inputMode == 'v'){
DVLevine 3:5696ac47658a 627 v_th = (float)atof(buffer_serial);
DVLevine 3:5696ac47658a 628 inputMode = 'n';
DVLevine 3:5696ac47658a 629 pc.printf("v_th is set to %f \r\n", v_th);
DVLevine 3:5696ac47658a 630 saveParam();
DVLevine 3:5696ac47658a 631 } else if(inputMode == 'f'){
DVLevine 3:5696ac47658a 632 flipMode = (float)atof(buffer_serial);
DVLevine 3:5696ac47658a 633 inputMode = 'n';
DVLevine 3:5696ac47658a 634 pc.printf("flipMode is set to %f \r\n", flipMode);
DVLevine 3:5696ac47658a 635 saveParam();
DVLevine 3:5696ac47658a 636 } else if(inputMode == 'g'){
DVLevine 3:5696ac47658a 637 torqueCol_d = (float)atof(buffer_serial);
DVLevine 3:5696ac47658a 638 inputMode = 'n';
DVLevine 3:5696ac47658a 639 pc.printf("torqueCol_d is set to %f \r\n", torqueCol_d);
DVLevine 3:5696ac47658a 640 saveParam();
DVLevine 3:5696ac47658a 641 } else if(inputMode == 'h'){
DVLevine 3:5696ac47658a 642 torqueFlight_d = (float)atof(buffer_serial);
DVLevine 3:5696ac47658a 643 inputMode = 'n';
DVLevine 3:5696ac47658a 644 pc.printf("torqueFlight_d is set to %f \r\n", torqueFlight_d);
DVLevine 3:5696ac47658a 645 saveParam();
DVLevine 3:5696ac47658a 646 }
DVLevine 3:5696ac47658a 647
DVLevine 3:5696ac47658a 648
DVLevine 3:5696ac47658a 649
DVLevine 3:5696ac47658a 650
DVLevine 3:5696ac47658a 651 if(buffer_serial[0] == 'c'){
DVLevine 3:5696ac47658a 652 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",
DVLevine 3:5696ac47658a 653 collisionDuration, Kp, Kd, theta_d, k_th, b_th, v_th, flipMode, torqueCol_d, torqueFlight_d);
kennakagaki 2:983a818c6ddf 654 }
DVLevine 3:5696ac47658a 655
kennakagaki 2:983a818c6ddf 656 }
kennakagaki 2:983a818c6ddf 657
kennakagaki 2:983a818c6ddf 658 /******************** LogData to SD card *****************************/
kennakagaki 2:983a818c6ddf 659
kennakagaki 2:983a818c6ddf 660 int trialTimeCount = 0;
kennakagaki 2:983a818c6ddf 661
DVLevine 3:5696ac47658a 662 const int logValNum = 16;
kennakagaki 2:983a818c6ddf 663 char logValName[logValNum][30];
DVLevine 3:5696ac47658a 664 float logVal[logValNum][2500];
kennakagaki 2:983a818c6ddf 665 int currentLogNum = 0;
DVLevine 3:5696ac47658a 666
kennakagaki 2:983a818c6ddf 667 void updateTrialTime(){
kennakagaki 2:983a818c6ddf 668 //trial time read
kennakagaki 2:983a818c6ddf 669 FILE *fp_tr = fopen("/sd/mydir/trialtime.txt", "r");
DVLevine 3:5696ac47658a 670
kennakagaki 2:983a818c6ddf 671 char Buffer_t[512];
DVLevine 3:5696ac47658a 672
kennakagaki 2:983a818c6ddf 673 if(fp_tr == NULL) { error("Could not open file for reading\r\n"); }
DVLevine 3:5696ac47658a 674
kennakagaki 2:983a818c6ddf 675 while(fgets (Buffer_t, 512, fp_tr) != NULL){
kennakagaki 2:983a818c6ddf 676 Buffer_t[strlen(Buffer_t)-1] = 0;
kennakagaki 2:983a818c6ddf 677 printf("String = \"%s\" \r\n", Buffer_t);
kennakagaki 2:983a818c6ddf 678 }
kennakagaki 2:983a818c6ddf 679 fclose(fp_tr);
DVLevine 3:5696ac47658a 680
kennakagaki 2:983a818c6ddf 681 trialTimeCount = (int)atof(Buffer_t);
DVLevine 3:5696ac47658a 682
kennakagaki 2:983a818c6ddf 683 printf("last trialTimeCount was %i. \n", trialTimeCount);
DVLevine 3:5696ac47658a 684
kennakagaki 2:983a818c6ddf 685 trialTimeCount++; //count up trial time
kennakagaki 2:983a818c6ddf 686 printf("current trialTimeCount updated to %i. \n", trialTimeCount);
DVLevine 3:5696ac47658a 687
kennakagaki 2:983a818c6ddf 688 FILE *fp3 = fopen("/sd/mydir/trialtime.txt", "w");
kennakagaki 2:983a818c6ddf 689 if(fp3 == NULL) {
kennakagaki 2:983a818c6ddf 690 error("Could not open file for write\n");
kennakagaki 2:983a818c6ddf 691 }
kennakagaki 2:983a818c6ddf 692 char buf[10];
DVLevine 3:5696ac47658a 693
kennakagaki 2:983a818c6ddf 694 sprintf(buf, "%d", trialTimeCount);
DVLevine 3:5696ac47658a 695
kennakagaki 2:983a818c6ddf 696 fprintf(fp3, buf);
kennakagaki 2:983a818c6ddf 697 fprintf(fp3, "\r\n");
DVLevine 3:5696ac47658a 698 fclose(fp3);
kennakagaki 2:983a818c6ddf 699 printf("trial time saved\n");
kennakagaki 2:983a818c6ddf 700 }
kennakagaki 2:983a818c6ddf 701
kennakagaki 2:983a818c6ddf 702 void logData(){ // log data time
DVLevine 3:5696ac47658a 703
DVLevine 3:5696ac47658a 704
kennakagaki 2:983a818c6ddf 705 logVal[0][currentLogNum] = t.read();
DVLevine 3:5696ac47658a 706 logVal[1][currentLogNum] = (int)bool_collision;
DVLevine 3:5696ac47658a 707 logVal[2][currentLogNum] = theta;
DVLevine 3:5696ac47658a 708 logVal[3][currentLogNum] = theta_dot;
DVLevine 3:5696ac47658a 709 logVal[4][currentLogNum] = dT;
DVLevine 3:5696ac47658a 710 logVal[5][currentLogNum] = i;
DVLevine 3:5696ac47658a 711 logVal[6][currentLogNum] = outputVal;
DVLevine 3:5696ac47658a 712 logVal[7][currentLogNum] = torque;
DVLevine 3:5696ac47658a 713 logVal[8][currentLogNum] = kalmanAngle;
DVLevine 3:5696ac47658a 714 logVal[9][currentLogNum] = accX;
DVLevine 3:5696ac47658a 715 logVal[10][currentLogNum] = accY;
DVLevine 3:5696ac47658a 716 logVal[11][currentLogNum] = accZ;
DVLevine 3:5696ac47658a 717 logVal[12][currentLogNum] = gyroX;
DVLevine 3:5696ac47658a 718 logVal[13][currentLogNum] = gyroY;
DVLevine 3:5696ac47658a 719 logVal[14][currentLogNum] = gyroZ;
DVLevine 3:5696ac47658a 720
DVLevine 4:227f0847a797 721 //printf("logged data for %i. t.read() = %f \r\n", currentLogNum, t.read());
DVLevine 3:5696ac47658a 722
DVLevine 3:5696ac47658a 723 currentLogNum++;
kennakagaki 2:983a818c6ddf 724 }
kennakagaki 2:983a818c6ddf 725
kennakagaki 2:983a818c6ddf 726 void saveData(){ // call when the while loop ends or the button pressed
kennakagaki 2:983a818c6ddf 727 sprintf( logValName[0], "time");
DVLevine 3:5696ac47658a 728 sprintf( logValName[1], "bool_collision");
DVLevine 3:5696ac47658a 729 sprintf( logValName[2], "theta");
DVLevine 3:5696ac47658a 730 sprintf( logValName[3], "theta_dot");
DVLevine 3:5696ac47658a 731 sprintf( logValName[4], "dT");
DVLevine 3:5696ac47658a 732 sprintf( logValName[5], "current");
DVLevine 3:5696ac47658a 733 sprintf( logValName[6], "outputVal");
DVLevine 3:5696ac47658a 734 sprintf( logValName[7], "torque");
DVLevine 3:5696ac47658a 735 sprintf( logValName[8], "kalmanAngle");
DVLevine 3:5696ac47658a 736 sprintf( logValName[9], "accX");
DVLevine 3:5696ac47658a 737 sprintf( logValName[10], "accY");
DVLevine 3:5696ac47658a 738 sprintf( logValName[11], "accZ");
DVLevine 3:5696ac47658a 739 sprintf( logValName[12], "gyroX");
DVLevine 3:5696ac47658a 740 sprintf( logValName[13], "gyroY");
DVLevine 3:5696ac47658a 741 sprintf( logValName[14], "gyroZ");
DVLevine 3:5696ac47658a 742
DVLevine 3:5696ac47658a 743
kennakagaki 2:983a818c6ddf 744 char filename[256];
DVLevine 3:5696ac47658a 745 sprintf(filename, "/sd/mydir/log/flipper_logData_%i.txt",trialTimeCount);
DVLevine 3:5696ac47658a 746
kennakagaki 2:983a818c6ddf 747 FILE *f_sv = fopen(filename, "w");
kennakagaki 2:983a818c6ddf 748 if(f_sv == NULL) {
kennakagaki 2:983a818c6ddf 749 error("Could not open file for write\n");
kennakagaki 2:983a818c6ddf 750 }
DVLevine 3:5696ac47658a 751
kennakagaki 2:983a818c6ddf 752 for(int i = 0; i < logValNum; i++){
kennakagaki 2:983a818c6ddf 753 fprintf(f_sv, logValName[i]);
kennakagaki 2:983a818c6ddf 754 fprintf(f_sv, ",");
kennakagaki 2:983a818c6ddf 755 }
kennakagaki 2:983a818c6ddf 756 fprintf(f_sv, "\r\n");
DVLevine 3:5696ac47658a 757
kennakagaki 2:983a818c6ddf 758 for(int j = 0; j < currentLogNum; j++){
kennakagaki 2:983a818c6ddf 759 for(int i = 0; i < logValNum; i++){
kennakagaki 2:983a818c6ddf 760 //char buf_temp[10];
kennakagaki 2:983a818c6ddf 761 //int a = 5;
DVLevine 3:5696ac47658a 762
DVLevine 3:5696ac47658a 763 char cVal[8];
DVLevine 3:5696ac47658a 764
kennakagaki 2:983a818c6ddf 765 sprintf(cVal,"%.3f", logVal[i][j]);
kennakagaki 2:983a818c6ddf 766
kennakagaki 2:983a818c6ddf 767 //sprintf(buf_temp, "%f", logVal[i][j]);
DVLevine 3:5696ac47658a 768
kennakagaki 2:983a818c6ddf 769 fprintf(f_sv, cVal);
kennakagaki 2:983a818c6ddf 770 fprintf(f_sv, ",");
kennakagaki 2:983a818c6ddf 771 }
kennakagaki 2:983a818c6ddf 772 fprintf(f_sv, "\r\n");
kennakagaki 2:983a818c6ddf 773 }
DVLevine 3:5696ac47658a 774
DVLevine 3:5696ac47658a 775
DVLevine 3:5696ac47658a 776 fclose(f_sv);
DVLevine 3:5696ac47658a 777
kennakagaki 2:983a818c6ddf 778 printf("Log Data file is saved as 'flipper_logData_%i.txt'.\n", trialTimeCount);
kennakagaki 2:983a818c6ddf 779 }
kennakagaki 2:983a818c6ddf 780
DVLevine 1:59124c69d0c3 781 /*******************************************************************/
DVLevine 1:59124c69d0c3 782
DVLevine 3:5696ac47658a 783 /******************** FLIP STRATEGIES *****************************/
DVLevine 4:227f0847a797 784
DVLevine 4:227f0847a797 785 float targetTheta = 0.0;
DVLevine 4:227f0847a797 786 float stiffnessStrategy(bool collision){
DVLevine 3:5696ac47658a 787 // depends on global variables
DVLevine 3:5696ac47658a 788 // theta_d: reference angle
DVLevine 3:5696ac47658a 789 // theta: measured angle at joint B
DVLevine 3:5696ac47658a 790 // theta_dot: measured angular velocity at joint B
DVLevine 4:227f0847a797 791 //if (collisionHappened<1){
DVLevine 4:227f0847a797 792 if (!collision){
DVLevine 4:227f0847a797 793 targetTheta = 0.0;
DVLevine 4:227f0847a797 794 }else{
DVLevine 4:227f0847a797 795 targetTheta = theta_d;
DVLevine 4:227f0847a797 796 }
DVLevine 3:5696ac47658a 797
DVLevine 4:227f0847a797 798 //printf("TARGET THETA %f\n",targetTheta);
DVLevine 4:227f0847a797 799
DVLevine 4:227f0847a797 800
DVLevine 4:227f0847a797 801 e = targetTheta-theta;
DVLevine 3:5696ac47658a 802 /*torque = -k_th*(theta-theta_d)-b_th*theta_dot+v_th*theta_dot;
DVLevine 3:5696ac47658a 803 if (torque<0){
DVLevine 3:5696ac47658a 804 torque = max(-1.4, torque);
DVLevine 3:5696ac47658a 805 }else{
DVLevine 3:5696ac47658a 806 torque = min(1.4, torque);
DVLevine 3:5696ac47658a 807 }
DVLevine 3:5696ac47658a 808 // printf("torque %f\n", torque);
DVLevine 3:5696ac47658a 809
DVLevine 4:227f0847a797 810 //i_d = torque/k_b;
DVLevine 4:227f0847a797 811 //i = readCurrent();
DVLevine 4:227f0847a797 812 //e = i_d-i;
DVLevine 4:227f0847a797 813 //outputVal = r*i_d+Kp*(e)-k_b*theta_dot;*/
DVLevine 3:5696ac47658a 814 //printf("%f\n",e);
DVLevine 3:5696ac47658a 815 outputVal = k_th*e+b_th*(-theta_dot);
DVLevine 3:5696ac47658a 816 //outputVal = Kp*e + Kd*(-theta_dot);
DVLevine 3:5696ac47658a 817 return outputVal;
DVLevine 3:5696ac47658a 818 }
DVLevine 3:5696ac47658a 819
DVLevine 3:5696ac47658a 820 float torqueStrategy(bool collision){
DVLevine 3:5696ac47658a 821 //uses global variables i,r
DVLevine 3:5696ac47658a 822 //strategy begins at collisions
DVLevine 3:5696ac47658a 823 if (collision){
DVLevine 3:5696ac47658a 824 torque = torqueCol_d; //predefined value
DVLevine 3:5696ac47658a 825 }else{
DVLevine 4:227f0847a797 826 if (hitTimes>0){
DVLevine 3:5696ac47658a 827 torque = torqueFlight_d;
DVLevine 4:227f0847a797 828 }
DVLevine 4:227f0847a797 829 else{
DVLevine 4:227f0847a797 830 torque = 0;
DVLevine 4:227f0847a797 831 }
DVLevine 3:5696ac47658a 832 }
DVLevine 3:5696ac47658a 833 // computation for output from torque
DVLevine 3:5696ac47658a 834 // where these torques are discerned from
DVLevine 3:5696ac47658a 835 // simulation profile
DVLevine 3:5696ac47658a 836 i_d = torque/k_b;
DVLevine 3:5696ac47658a 837 i = readCurrent();
DVLevine 3:5696ac47658a 838 e = i_d-i;
DVLevine 4:227f0847a797 839 //printf("%f\n",outputVal);
DVLevine 4:227f0847a797 840 if (torque==0){
DVLevine 4:227f0847a797 841 outputVal=0;
DVLevine 4:227f0847a797 842 }
DVLevine 4:227f0847a797 843 else{
DVLevine 3:5696ac47658a 844 outputVal = r*i_d+Kp*(e)-k_b*theta_dot;
DVLevine 4:227f0847a797 845 }
DVLevine 3:5696ac47658a 846 return outputVal;
DVLevine 3:5696ac47658a 847 }
DVLevine 3:5696ac47658a 848
DVLevine 3:5696ac47658a 849 float stiffnessAndtorqueStrategy(bool collisionState){
DVLevine 4:227f0847a797 850 return stiffnessStrategy(collisionState)+torqueStrategy(collisionState);
DVLevine 3:5696ac47658a 851 }
DVLevine 3:5696ac47658a 852
DVLevine 3:5696ac47658a 853 float executeFlipStrategy(int flipmode, bool collisionState){
DVLevine 3:5696ac47658a 854
DVLevine 3:5696ac47658a 855 // Mode 1: Stiffness only
DVLevine 3:5696ac47658a 856 if (flipmode==1){
DVLevine 4:227f0847a797 857 return stiffnessStrategy(collisionState);
DVLevine 3:5696ac47658a 858 }else{
DVLevine 3:5696ac47658a 859 // Mode 2: Torque Profile only
DVLevine 3:5696ac47658a 860 if (flipmode==2){
DVLevine 3:5696ac47658a 861 return torqueStrategy(collisionState);
DVLevine 3:5696ac47658a 862 }
DVLevine 3:5696ac47658a 863 // Mode 3: Stiffness and Torque
DVLevine 3:5696ac47658a 864 else{
DVLevine 3:5696ac47658a 865 return stiffnessAndtorqueStrategy(collisionState);
DVLevine 3:5696ac47658a 866 }
DVLevine 3:5696ac47658a 867 }
DVLevine 3:5696ac47658a 868
DVLevine 3:5696ac47658a 869 }
kennakagaki 2:983a818c6ddf 870
kennakagaki 2:983a818c6ddf 871
DVLevine 1:59124c69d0c3 872 /******************** EXECUTE MOTOR LOOP *****************************/
DVLevine 1:59124c69d0c3 873
DVLevine 1:59124c69d0c3 874 float pikeAngle = 0.0;
DVLevine 1:59124c69d0c3 875
DVLevine 1:59124c69d0c3 876 /** Core Motor Code Loop**/
DVLevine 1:59124c69d0c3 877 void executeMotorLoop(){
DVLevine 1:59124c69d0c3 878
DVLevine 1:59124c69d0c3 879 printf("Entering Motor Loop\n");
DVLevine 1:59124c69d0c3 880 // initial setup
DVLevine 1:59124c69d0c3 881 t.reset();
DVLevine 1:59124c69d0c3 882 t.start();
DVLevine 1:59124c69d0c3 883 encoder.reset();
DVLevine 1:59124c69d0c3 884 setMotorDir('f'); //begin with motor driving forward
DVLevine 1:59124c69d0c3 885 motorPWM.write(0);
DVLevine 1:59124c69d0c3 886 lastTime=t.read();
DVLevine 3:5696ac47658a 887 startTimeReference = t.read(); //set reference time for Kalman Angle
DVLevine 1:59124c69d0c3 888
DVLevine 3:5696ac47658a 889 /* Run experiment */
DVLevine 3:5696ac47658a 890 while( t.read() < 10.0 && !debugModeOn) {
DVLevine 3:5696ac47658a 891 //Update IMU and Kalman
DVLevine 3:5696ac47658a 892 readIMU();
DVLevine 1:59124c69d0c3 893
DVLevine 3:5696ac47658a 894 //read present time and calculate dt
DVLevine 3:5696ac47658a 895 presentTime = t.read();
DVLevine 3:5696ac47658a 896 dT = presentTime - lastTime;
DVLevine 1:59124c69d0c3 897
DVLevine 3:5696ac47658a 898 // set last time to determine next time step
DVLevine 3:5696ac47658a 899 lastTime = presentTime;
DVLevine 3:5696ac47658a 900
DVLevine 3:5696ac47658a 901 // Perform control loop logic
DVLevine 3:5696ac47658a 902 theta = getAngleFromPulses(encoder.getPulses());
DVLevine 3:5696ac47658a 903 theta_dot = getAngleFromPulses(encoder.getVelocity());
DVLevine 1:59124c69d0c3 904
DVLevine 3:5696ac47658a 905 /* Checking collisions */
DVLevine 1:59124c69d0c3 906
DVLevine 3:5696ac47658a 907 // Determines whether new hit.
DVLevine 4:227f0847a797 908 if ((!bool_collision) && checkCollision(accX,dT) && (hitTimes<1)){
DVLevine 4:227f0847a797 909 printf("i am dumb\n");
DVLevine 4:227f0847a797 910 collisionTime = t.read();
DVLevine 4:227f0847a797 911 collisionTimeRemaining = collisionDuration;
DVLevine 4:227f0847a797 912 bool_collision = true;
DVLevine 4:227f0847a797 913 hitTimes = hitTimes + 1;
DVLevine 3:5696ac47658a 914 }
DVLevine 3:5696ac47658a 915 // If not a new hit, see if we are still in "hit state" from
DVLevine 3:5696ac47658a 916 // collision duration parameter for actuation
DVLevine 3:5696ac47658a 917 else{
DVLevine 4:227f0847a797 918 if ((bool_collision)&&(hitTimes<=1)){
DVLevine 4:227f0847a797 919 collisionTimeRemaining = (max((collisionDuration)-(t.read()-collisionTime),0.0));
DVLevine 4:227f0847a797 920 if (collisionTimeRemaining<=0.0){
DVLevine 4:227f0847a797 921 bool_collision = false;
DVLevine 4:227f0847a797 922 }
DVLevine 4:227f0847a797 923 }
DVLevine 4:227f0847a797 924 }
DVLevine 4:227f0847a797 925 //bool_collision = checkCollision(accX,dT);
DVLevine 4:227f0847a797 926
DVLevine 1:59124c69d0c3 927
DVLevine 3:5696ac47658a 928 /* actuation strategy */
DVLevine 3:5696ac47658a 929 // flip mode 1: stiffness only
DVLevine 3:5696ac47658a 930 // flip mode 2: torque driving only
DVLevine 3:5696ac47658a 931 // flip mode 3: torque driving with stiffness
DVLevine 1:59124c69d0c3 932
DVLevine 1:59124c69d0c3 933
DVLevine 3:5696ac47658a 934 // Initial State, zero torque or other affects.
DVLevine 3:5696ac47658a 935 torque = 0.0;
DVLevine 1:59124c69d0c3 936
DVLevine 3:5696ac47658a 937 /* Compute Actuation Force */
DVLevine 3:5696ac47658a 938 outputVal = executeFlipStrategy(flipMode, bool_collision);
DVLevine 1:59124c69d0c3 939
DVLevine 3:5696ac47658a 940 /* Actuate Motors */
DVLevine 3:5696ac47658a 941 if (outputVal<0){
DVLevine 3:5696ac47658a 942 //negative difference, need to move motor backward to correct
DVLevine 3:5696ac47658a 943 setMotorDir('f');
DVLevine 3:5696ac47658a 944 }
DVLevine 3:5696ac47658a 945 if (outputVal>0){
DVLevine 3:5696ac47658a 946 //positive difference, need to move motor forward to correct
DVLevine 3:5696ac47658a 947 setMotorDir('r');
DVLevine 3:5696ac47658a 948 }
DVLevine 3:5696ac47658a 949 motorPWM.write(toDutyCycle(abs(outputVal)));
DVLevine 3:5696ac47658a 950 //printf("Serial: %f\n",outputVal);
DVLevine 1:59124c69d0c3 951
DVLevine 3:5696ac47658a 952 /* Log Data */
DVLevine 3:5696ac47658a 953 logData();
DVLevine 3:5696ac47658a 954 if(!saveDataButton){
DVLevine 3:5696ac47658a 955 saveData();
DVLevine 3:5696ac47658a 956 debugModeOn = true;
DVLevine 3:5696ac47658a 957 }
DVLevine 3:5696ac47658a 958
DVLevine 3:5696ac47658a 959 }
DVLevine 3:5696ac47658a 960 // End of action cycle, turn motor "off"
DVLevine 3:5696ac47658a 961 motorPWM.write(0);
DVLevine 0:73a369b7b5b4 962 }
DVLevine 1:59124c69d0c3 963
kennakagaki 2:983a818c6ddf 964
kennakagaki 2:983a818c6ddf 965
kennakagaki 2:983a818c6ddf 966
DVLevine 1:59124c69d0c3 967 /****************************** MAIN *************************************/
DVLevine 1:59124c69d0c3 968
DVLevine 1:59124c69d0c3 969 int main()
DVLevine 1:59124c69d0c3 970 {
DVLevine 3:5696ac47658a 971
DVLevine 3:5696ac47658a 972 printf("test!!\n");
DVLevine 3:5696ac47658a 973 updateTrialTime();
DVLevine 3:5696ac47658a 974 loadParam();
DVLevine 3:5696ac47658a 975
DVLevine 3:5696ac47658a 976 /* setup imu and kalman filter for rotation */
DVLevine 3:5696ac47658a 977 setupIMU();
DVLevine 3:5696ac47658a 978 setupKalman();
DVLevine 3:5696ac47658a 979
DVLevine 3:5696ac47658a 980 /* run core motor loop */
DVLevine 3:5696ac47658a 981 executeMotorLoop();
DVLevine 1:59124c69d0c3 982
DVLevine 3:5696ac47658a 983 /* save trial data to sd card */
DVLevine 3:5696ac47658a 984 saveData();
DVLevine 1:59124c69d0c3 985
DVLevine 3:5696ac47658a 986 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",
DVLevine 3:5696ac47658a 987 collisionDuration, Kp, Kd, theta_d, k_th, b_th, v_th, flipMode, torqueCol_d, torqueFlight_d);
DVLevine 3:5696ac47658a 988 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'.");
DVLevine 1:59124c69d0c3 989
DVLevine 3:5696ac47658a 990 while(1){ // debugMode
DVLevine 3:5696ac47658a 991 serialUpdateVal();
DVLevine 3:5696ac47658a 992 }
DVLevine 1:59124c69d0c3 993
DVLevine 3:5696ac47658a 994 }