Pike Flippers / Mbed 2 deprecated Pike_the_Flipper_Main_Branch

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BMI160 QEI_pmw SDFileSystem USBDevice kalman max32630fthr mbed

Fork of Pike_the_Flipper_Main_Branch by Daniel Levine

main.cpp@2:983a818c6ddf, 2017-12-02 (annotated)

Committer:
kennakagaki
Date:
Sat Dec 02 23:33:17 2017 +0000
Revision:
2:983a818c6ddf
Parent:
1:59124c69d0c3
Child:
3:5696ac47658a
1. Log Data and Save with SD card; 2. Load and Save Parameters with SD card and update with Serial.

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 0:73a369b7b5b4 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 1:59124c69d0c3 16 PwmOut motorPWM(P4_0); // Motor PWM output
DVLevine 1:59124c69d0c3 17 DigitalOut motorFwd(P5_6); // Motor forward enable
DVLevine 1:59124c69d0c3 18 DigitalOut motorRev(P5_5); // Motor backward enable
DVLevine 1:59124c69d0c3 19
DVLevine 1:59124c69d0c3 20 //Analog input
DVLevine 1:59124c69d0c3 21 AnalogIn ain(AIN_5);
DVLevine 1:59124c69d0c3 22 Timer t; // Timer to measure elapsed time of experiment
DVLevine 1:59124c69d0c3 23 QEI encoder(P5_3,P5_4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding
DVLevine 1:59124c69d0c3 24
DVLevine 1:59124c69d0c3 25 DigitalOut led1(LED1, 1);
DVLevine 1:59124c69d0c3 26
kennakagaki 2:983a818c6ddf 27 // added by ken //
kennakagaki 2:983a818c6ddf 28 DigitalIn saveDataButton(P2_3);
kennakagaki 2:983a818c6ddf 29 SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // the pinout on the mbed Cool Components workshop board
kennakagaki 2:983a818c6ddf 30 Serial pc(USBTX, USBRX);
kennakagaki 2:983a818c6ddf 31 // added by ken - ends here //
kennakagaki 2:983a818c6ddf 32
kennakagaki 2:983a818c6ddf 33
DVLevine 1:59124c69d0c3 34 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
DVLevine 1:59124c69d0c3 35 I2C i2cBus_acc(P5_7, P6_0);
DVLevine 1:59124c69d0c3 36 BMI160_I2C* m_imu;//(i2cBus_acc, BMI160_I2C::I2C_ADRS_SDO_LO);
DVLevine 1:59124c69d0c3 37 BMI160::AccConfig accConfig;
DVLevine 1:59124c69d0c3 38 BMI160::GyroConfig gyroConfig;
DVLevine 1:59124c69d0c3 39 uint32_t failures = 0;
DVLevine 1:59124c69d0c3 40
DVLevine 1:59124c69d0c3 41 // for storing values
DVLevine 1:59124c69d0c3 42 float imuTemperature;
DVLevine 1:59124c69d0c3 43 BMI160::SensorData accData;
DVLevine 1:59124c69d0c3 44 BMI160::SensorData gyroData;
DVLevine 1:59124c69d0c3 45 BMI160::SensorTime sensorTime;
DVLevine 1:59124c69d0c3 46
DVLevine 1:59124c69d0c3 47 // IMU Methods
DVLevine 1:59124c69d0c3 48 float accX = 0.0;
DVLevine 1:59124c69d0c3 49 float accY = 0.0;
DVLevine 1:59124c69d0c3 50 float accZ = 0.0;
DVLevine 1:59124c69d0c3 51
DVLevine 1:59124c69d0c3 52 float gyroX = 0.0;
DVLevine 1:59124c69d0c3 53 float gyroY = 0.0;
DVLevine 1:59124c69d0c3 54 float gyroZ = 0.0;
DVLevine 1:59124c69d0c3 55
DVLevine 1:59124c69d0c3 56 // Collision Detection Variables - originally only for single collisions
DVLevine 1:59124c69d0c3 57 bool bool_collision = false; // collision state
DVLevine 1:59124c69d0c3 58 float collisionTime = 0.0; //time collision occurred
DVLevine 1:59124c69d0c3 59 float collisionTimeRemaining = 0.0; //variable to hold bool_collision for set time
DVLevine 1:59124c69d0c3 60 float collisionDuration = 0.5; // set as variable denoted collision duration.
DVLevine 1:59124c69d0c3 61 // the duration of actuation
DVLevine 1:59124c69d0c3 62
DVLevine 1:59124c69d0c3 63 /****************** MOTOR CONTROL VARIABLES FOR STATES **********************/
DVLevine 1:59124c69d0c3 64 //For motor control
DVLevine 1:59124c69d0c3 65 //Variables for states
DVLevine 1:59124c69d0c3 66 float theta_d = 0.0;
DVLevine 1:59124c69d0c3 67 //float Kp = 0.0;
DVLevine 1:59124c69d0c3 68 float Kd = 1.2; //made up
DVLevine 1:59124c69d0c3 69 float Ki = 0.1; //made up
DVLevine 1:59124c69d0c3 70
DVLevine 1:59124c69d0c3 71 //Declare Parameters
DVLevine 1:59124c69d0c3 72 float e = 0.0; //error, theta_d-theta
DVLevine 1:59124c69d0c3 73 float theta = 0.0; //present theta
DVLevine 1:59124c69d0c3 74 float theta_dot = 0.0; //present angular velocity
DVLevine 1:59124c69d0c3 75 float val = 0.0; //
DVLevine 1:59124c69d0c3 76
DVLevine 1:59124c69d0c3 77 float lastTime = 0.0; //last time step
DVLevine 1:59124c69d0c3 78 float presentTime = 0.0; //current time step
DVLevine 1:59124c69d0c3 79 float dT = 0.0; //time difference
DVLevine 1:59124c69d0c3 80
DVLevine 1:59124c69d0c3 81 /** Set gain parameters using bluetooth **/
DVLevine 1:59124c69d0c3 82 //Default Gains set to avoid problems
DVLevine 1:59124c69d0c3 83 float i_d = 0; //target current % changed by code
DVLevine 1:59124c69d0c3 84 float Kp = 3.0; // proportional gain
DVLevine 1:59124c69d0c3 85 float r = 3.2; // winding resistance;
DVLevine 1:59124c69d0c3 86 float k_b = 0.23; // back emf
DVLevine 1:59124c69d0c3 87 float k_th = 4; // proportional theta gain for torque
DVLevine 1:59124c69d0c3 88 float b_th = 0.2185; // proportional theta_dot gain for damping
DVLevine 1:59124c69d0c3 89 float v_th = 0.0185;
DVLevine 1:59124c69d0c3 90
kennakagaki 2:983a818c6ddf 91
DVLevine 1:59124c69d0c3 92 float i = 0.0; // present current
DVLevine 1:59124c69d0c3 93 float e_accum = 0.0;
DVLevine 1:59124c69d0c3 94
DVLevine 1:59124c69d0c3 95
DVLevine 1:59124c69d0c3 96 float torque = 0.0; //
DVLevine 1:59124c69d0c3 97 float outputVal;//final compensation value (in rad) --> converted into dutycycle
DVLevine 1:59124c69d0c3 98
kennakagaki 2:983a818c6ddf 99
kennakagaki 2:983a818c6ddf 100 bool debugModeOn = false;
kennakagaki 2:983a818c6ddf 101
DVLevine 1:59124c69d0c3 102 /********************** FEEDBACK UTILITY FUNCTIONS ******************************/
DVLevine 1:59124c69d0c3 103 // A min function (as in min(a,b) or max(a,b)))
DVLevine 1:59124c69d0c3 104 float min(float a, float b){
DVLevine 1:59124c69d0c3 105 return a<b ? a : b;
DVLevine 1:59124c69d0c3 106 }
DVLevine 1:59124c69d0c3 107
DVLevine 1:59124c69d0c3 108 float max(float a, float b){
DVLevine 1:59124c69d0c3 109 return a>b ? a : b;
DVLevine 1:59124c69d0c3 110 }
DVLevine 1:59124c69d0c3 111 // for final angle to pwm duty cycle calc
DVLevine 1:59124c69d0c3 112 float toDutyCycle(float inputRad){
DVLevine 1:59124c69d0c3 113 float temp = abs(inputRad);
DVLevine 1:59124c69d0c3 114 if (temp>12){
DVLevine 1:59124c69d0c3 115 return 1.0;
DVLevine 1:59124c69d0c3 116 }else{
DVLevine 1:59124c69d0c3 117 return temp/12.0;
DVLevine 1:59124c69d0c3 118 }
DVLevine 1:59124c69d0c3 119 //abs(min(12.0,inputRad)/12.0);
DVLevine 1:59124c69d0c3 120 }
DVLevine 1:59124c69d0c3 121 // Calculates angles in radians from position
DVLevine 1:59124c69d0c3 122 float getAngleFromPulses(int input){
DVLevine 1:59124c69d0c3 123 return input*(2*M_PI)/1200.0;
DVLevine 1:59124c69d0c3 124 }
DVLevine 1:59124c69d0c3 125 // Set motor direction
DVLevine 1:59124c69d0c3 126 void setMotorDir(char dir){
DVLevine 1:59124c69d0c3 127 if (dir=='f'){
DVLevine 1:59124c69d0c3 128 motorFwd = 1;
DVLevine 1:59124c69d0c3 129 motorRev = 0;
DVLevine 1:59124c69d0c3 130 }else{
DVLevine 1:59124c69d0c3 131 motorFwd = 0;
DVLevine 1:59124c69d0c3 132 motorRev = 1;
DVLevine 1:59124c69d0c3 133 }
DVLevine 1:59124c69d0c3 134 }
DVLevine 1:59124c69d0c3 135
DVLevine 1:59124c69d0c3 136 // gets current
DVLevine 1:59124c69d0c3 137 float readCurrent(){
DVLevine 1:59124c69d0c3 138 return 36.7*ain.read()*(1.815)-18.3;
DVLevine 1:59124c69d0c3 139 // return 36.7*ain.read()*(1.815)-18.3;
DVLevine 1:59124c69d0c3 140 }
DVLevine 1:59124c69d0c3 141
DVLevine 1:59124c69d0c3 142 /********************* LED DISPLAY UTILITY FUNCTIONS *************************/
DVLevine 1:59124c69d0c3 143 PwmOut rLED(LED1);
DVLevine 1:59124c69d0c3 144 PwmOut gLED(LED2);
DVLevine 1:59124c69d0c3 145 PwmOut bLED(LED3);
DVLevine 1:59124c69d0c3 146
DVLevine 1:59124c69d0c3 147 void updateLEDS(float b1,float b2,float b3){
DVLevine 1:59124c69d0c3 148
DVLevine 1:59124c69d0c3 149 if(!bool_collision){
DVLevine 1:59124c69d0c3 150 rLED.write(max(1-b1/4,0.0));
DVLevine 1:59124c69d0c3 151 gLED.write(max(1-b2/4,0.0));
DVLevine 1:59124c69d0c3 152 bLED.write(max(1-b3/4,0.0));
DVLevine 1:59124c69d0c3 153 }else{
DVLevine 1:59124c69d0c3 154 rLED.write(0);
DVLevine 1:59124c69d0c3 155 gLED.write(0);
DVLevine 1:59124c69d0c3 156 bLED.write(1);
DVLevine 1:59124c69d0c3 157 }
DVLevine 1:59124c69d0c3 158 }
DVLevine 1:59124c69d0c3 159
DVLevine 1:59124c69d0c3 160 /********************* COLLISION UTILITY FUNCTIONS *************************/
DVLevine 1:59124c69d0c3 161 float last_xAcc = 0.0;
DVLevine 1:59124c69d0c3 162
DVLevine 1:59124c69d0c3 163 bool checkCollision(float xAccel,float dT){
DVLevine 1:59124c69d0c3 164 //printf("%f\n",xAccel);
DVLevine 1:59124c69d0c3 165 //if ((xAccel > 1.85) || (xAccel <-1.85) || (collisionTimeRemaining>0)){
DVLevine 1:59124c69d0c3 166 //if (last_xAcc<0 && xAccel>0){
DVLevine 1:59124c69d0c3 167 float floatme = (xAccel-last_xAcc)/dT;
DVLevine 1:59124c69d0c3 168 if (floatme>200.0){
DVLevine 1:59124c69d0c3 169 //printf("Collision! %f\n",xAccel);
DVLevine 1:59124c69d0c3 170 collisionTimeRemaining = max((collisionDuration)-(t.read()-collisionTime),0.0);
DVLevine 1:59124c69d0c3 171 /* if ((xAccel>1.85) || xAccel <-1.85){
DVLevine 1:59124c69d0c3 172 collisionTimeRemaining = collisionDuration;
DVLevine 1:59124c69d0c3 173 }*/
DVLevine 1:59124c69d0c3 174 return true;
DVLevine 1:59124c69d0c3 175 }else{
DVLevine 1:59124c69d0c3 176 return false;
DVLevine 1:59124c69d0c3 177 }
DVLevine 1:59124c69d0c3 178 }
DVLevine 1:59124c69d0c3 179
DVLevine 1:59124c69d0c3 180 //unused function for now
DVLevine 1:59124c69d0c3 181 float collisionActuation(float duration){
DVLevine 1:59124c69d0c3 182 //first decide start time
DVLevine 1:59124c69d0c3 183 //then actuate for a set duration
DVLevine 1:59124c69d0c3 184 return duration;
DVLevine 1:59124c69d0c3 185 }
DVLevine 1:59124c69d0c3 186
DVLevine 1:59124c69d0c3 187
DVLevine 1:59124c69d0c3 188
DVLevine 1:59124c69d0c3 189 /*****************************************************************************/
DVLevine 1:59124c69d0c3 190
DVLevine 1:59124c69d0c3 191 /******************** SETUP IMU *****************************/
DVLevine 1:59124c69d0c3 192 kalman filterRotation;
DVLevine 1:59124c69d0c3 193 double refAngle = 0.0;
DVLevine 1:59124c69d0c3 194 double qGyro = 0.0;
DVLevine 1:59124c69d0c3 195 double qAngle = 0.0;
DVLevine 1:59124c69d0c3 196
DVLevine 1:59124c69d0c3 197 void setupKalman(){
DVLevine 1:59124c69d0c3 198 kalman_init(&filterRotation, R_matrix,Q_Gyro_matrix,Q_Accel_matrix);
DVLevine 1:59124c69d0c3 199 }
DVLevine 1:59124c69d0c3 200
DVLevine 1:59124c69d0c3 201
DVLevine 1:59124c69d0c3 202 void setupIMU()
DVLevine 1:59124c69d0c3 203 {
DVLevine 1:59124c69d0c3 204 i2cBus_acc.frequency(400000);
DVLevine 1:59124c69d0c3 205 m_imu = new BMI160_I2C(i2cBus_acc, BMI160_I2C::I2C_ADRS_SDO_LO);
DVLevine 1:59124c69d0c3 206 printf("\033[H"); //home
DVLevine 1:59124c69d0c3 207 printf("\033[0J"); //erase from cursor to end of screen
DVLevine 1:59124c69d0c3 208 if(m_imu->setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
DVLevine 1:59124c69d0c3 209 printf("Failed to set gyroscope power mode\n");
DVLevine 1:59124c69d0c3 210 failures++;
DVLevine 1:59124c69d0c3 211 }
DVLevine 1:59124c69d0c3 212 wait_ms(100);
DVLevine 1:59124c69d0c3 213
DVLevine 1:59124c69d0c3 214 if(m_imu->setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
DVLevine 1:59124c69d0c3 215 printf("Failed to set accelerometer power mode\n");
DVLevine 1:59124c69d0c3 216 failures++;
DVLevine 1:59124c69d0c3 217 }
DVLevine 1:59124c69d0c3 218 wait_ms(100);
DVLevine 1:59124c69d0c3 219
DVLevine 1:59124c69d0c3 220
DVLevine 1:59124c69d0c3 221 //example of setting user defined configuration
DVLevine 1:59124c69d0c3 222 accConfig.range = BMI160::SENS_4G;
DVLevine 1:59124c69d0c3 223 accConfig.us = BMI160::ACC_US_OFF;
DVLevine 1:59124c69d0c3 224 accConfig.bwp = BMI160::ACC_BWP_2;
DVLevine 1:59124c69d0c3 225 accConfig.odr = BMI160::ACC_ODR_12; //reads at 160 kHz
DVLevine 1:59124c69d0c3 226
DVLevine 1:59124c69d0c3 227 if(m_imu->getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) {
DVLevine 1:59124c69d0c3 228 printf("ACC Range = %d\n", accConfig.range);
DVLevine 1:59124c69d0c3 229 printf("ACC UnderSampling = %d\n", accConfig.us);
DVLevine 1:59124c69d0c3 230 printf("ACC BandWidthParam = %d\n", accConfig.bwp);
DVLevine 1:59124c69d0c3 231 printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
DVLevine 1:59124c69d0c3 232 } else {
DVLevine 1:59124c69d0c3 233 printf("Failed to get accelerometer configuration\n");
DVLevine 1:59124c69d0c3 234 failures++;
DVLevine 1:59124c69d0c3 235 }
DVLevine 1:59124c69d0c3 236
DVLevine 1:59124c69d0c3 237 if(m_imu->getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) {
DVLevine 1:59124c69d0c3 238 printf("GYRO Range = %d\n", gyroConfig.range);
DVLevine 1:59124c69d0c3 239 printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
DVLevine 1:59124c69d0c3 240 printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
DVLevine 1:59124c69d0c3 241 } else {
DVLevine 1:59124c69d0c3 242 printf("Failed to get gyroscope configuration\n");
DVLevine 1:59124c69d0c3 243 failures++;
DVLevine 0:73a369b7b5b4 244 }
DVLevine 1:59124c69d0c3 245
DVLevine 1:59124c69d0c3 246 wait(1.0);
DVLevine 1:59124c69d0c3 247
DVLevine 1:59124c69d0c3 248 m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
DVLevine 1:59124c69d0c3 249 m_imu->getTemperature(&imuTemperature);
DVLevine 1:59124c69d0c3 250
DVLevine 1:59124c69d0c3 251 accX = accData.xAxis.scaled;
DVLevine 1:59124c69d0c3 252 accY = accData.yAxis.scaled;
DVLevine 1:59124c69d0c3 253 accZ = accData.zAxis.scaled;
DVLevine 1:59124c69d0c3 254
DVLevine 1:59124c69d0c3 255 printf("ACC xAxis = %s%4.3f\n", "\033[K", accX);
DVLevine 1:59124c69d0c3 256 printf("ACC xAxis = %s%4.3f\n", "\033[K", accY);
DVLevine 1:59124c69d0c3 257 printf("ACC xAxis = %s%4.3f\n", "\033[K", accZ);
DVLevine 1:59124c69d0c3 258
DVLevine 1:59124c69d0c3 259
DVLevine 1:59124c69d0c3 260 updateLEDS(accX,accY,accZ);
DVLevine 1:59124c69d0c3 261 }
DVLevine 1:59124c69d0c3 262
DVLevine 1:59124c69d0c3 263 /******************** READ IMU *****************************/
DVLevine 1:59124c69d0c3 264
DVLevine 1:59124c69d0c3 265 // ▯ x is up, y is left, z is out of the board (towards me). BLE module is on top.
DVLevine 1:59124c69d0c3 266 // R i = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
DVLevine 1:59124c69d0c3 267 void getKalmanPrediction(double dT, float gyroReading, float accReading, float R){
DVLevine 1:59124c69d0c3 268 kalman_predict(&filterRotation, gyroReading, dT);
DVLevine 1:59124c69d0c3 269 kalman_update(&filterRotation, acos(accReading/R));
DVLevine 1:59124c69d0c3 270 }
DVLevine 1:59124c69d0c3 271
DVLevine 1:59124c69d0c3 272
DVLevine 1:59124c69d0c3 273 float lastKalTime = 0.0;
DVLevine 1:59124c69d0c3 274 float R = 0.0;
DVLevine 1:59124c69d0c3 275
DVLevine 1:59124c69d0c3 276 void readIMU()
DVLevine 1:59124c69d0c3 277 {
DVLevine 1:59124c69d0c3 278 m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
DVLevine 1:59124c69d0c3 279 m_imu->getTemperature(&imuTemperature);
DVLevine 1:59124c69d0c3 280
DVLevine 1:59124c69d0c3 281 //printf("ACC xAxis = %s%4.3f\n", "\033[K", accData.xAxis.scaled);
DVLevine 1:59124c69d0c3 282 //printf("ACC yAxis = %s%4.3f\n", "\033[K", accData.yAxis.scaled);
DVLevine 1:59124c69d0c3 283 //printf("ACC zAxis = %s%4.3f\n\n", "\033[K", accData.zAxis.scaled);
DVLevine 1:59124c69d0c3 284
DVLevine 1:59124c69d0c3 285 // printf("GYRO xAxis = %s%5.1f\n", "\033[K", gyroData.xAxis.scaled);
DVLevine 1:59124c69d0c3 286 // printf("GYRO yAxis = %s%5.1f\n", "\033[K", gyroData.yAxis.scaled);
DVLevine 1:59124c69d0c3 287 // printf("GYRO zAxis = %s%5.1f\n\n", "\033[K", gyroData.zAxis.scaled);
DVLevine 1:59124c69d0c3 288
DVLevine 1:59124c69d0c3 289 // printf("Sensor Time = %s%f\n", "\033[K", sensorTime.seconds);
DVLevine 1:59124c69d0c3 290 // printf("Sensor Temperature = %s%5.3f\n", "\033[K", imuTemperature);
DVLevine 1:59124c69d0c3 291
DVLevine 1:59124c69d0c3 292 accX = accData.xAxis.scaled;
DVLevine 1:59124c69d0c3 293 accY = accData.yAxis.scaled;
DVLevine 1:59124c69d0c3 294 accZ = accData.zAxis.scaled;
DVLevine 1:59124c69d0c3 295
DVLevine 1:59124c69d0c3 296 gyroX = gyroData.xAxis.scaled;
DVLevine 1:59124c69d0c3 297 gyroY = gyroData.yAxis.scaled;
DVLevine 1:59124c69d0c3 298 gyroZ = gyroData.zAxis.scaled;
DVLevine 1:59124c69d0c3 299
DVLevine 1:59124c69d0c3 300 //printf("%4.3f\n", accX);
DVLevine 1:59124c69d0c3 301
DVLevine 1:59124c69d0c3 302 //printf("ACC xAxis = %s%4.3f\n", "\033[K", accX);
DVLevine 1:59124c69d0c3 303 //printf("ACC yAxis = %s%4.3f\n", "\033[K", accY);
DVLevine 1:59124c69d0c3 304 //printf("ACC zAxis = %s%4.3f\n\n", "\033[K", accZ);
DVLevine 1:59124c69d0c3 305 R = sqrt(std::pow(accX, 2) + std::pow(accY, 2) + std::pow(accZ, 2));
DVLevine 1:59124c69d0c3 306 getKalmanPrediction(t.read()-lastKalTime, gyroY, accY, R);
DVLevine 1:59124c69d0c3 307 //printf("%f\n",kalman_get_angle(&filterRotation));
DVLevine 1:59124c69d0c3 308 lastKalTime=t.read();
DVLevine 1:59124c69d0c3 309
DVLevine 1:59124c69d0c3 310 updateLEDS(accX,accY,accZ);
DVLevine 1:59124c69d0c3 311 }
kennakagaki 2:983a818c6ddf 312
kennakagaki 2:983a818c6ddf 313 /*********** Load and Update and Save Parameters ***************/
kennakagaki 2:983a818c6ddf 314 char inputMode = 'n';
kennakagaki 2:983a818c6ddf 315
kennakagaki 2:983a818c6ddf 316 void loadParam(){ //from SD
kennakagaki 2:983a818c6ddf 317 //kp read
kennakagaki 2:983a818c6ddf 318 FILE *fp_kp = fopen("/sd/mydir/param/kp.txt", "r");
kennakagaki 2:983a818c6ddf 319 char Buf_p[30];
kennakagaki 2:983a818c6ddf 320 if(fp_kp == NULL) { error("Could not open file for reading\r\n"); }
kennakagaki 2:983a818c6ddf 321 while(fgets (Buf_p, 30, fp_kp) != NULL){
kennakagaki 2:983a818c6ddf 322 Buf_p[strlen(Buf_p)-1] = 0;
kennakagaki 2:983a818c6ddf 323 printf("String = \"%s\" \r\n", Buf_p);
kennakagaki 2:983a818c6ddf 324 }
kennakagaki 2:983a818c6ddf 325 fclose(fp_kp);
kennakagaki 2:983a818c6ddf 326
kennakagaki 2:983a818c6ddf 327 Kp = (float)atof(Buf_p);
kennakagaki 2:983a818c6ddf 328 printf("Kp is loaded as %f. \r\n", Kp);
kennakagaki 2:983a818c6ddf 329
kennakagaki 2:983a818c6ddf 330 //kd read
kennakagaki 2:983a818c6ddf 331 FILE *fp_kd = fopen("/sd/mydir/param/kd.txt", "r");
kennakagaki 2:983a818c6ddf 332 char Buf_d[30];
kennakagaki 2:983a818c6ddf 333 if(fp_kd == NULL) { error("Could not open file for reading\r\n"); }
kennakagaki 2:983a818c6ddf 334 while(fgets (Buf_d, 30, fp_kd) != NULL){
kennakagaki 2:983a818c6ddf 335 Buf_d[strlen(Buf_d)-1] = 0;
kennakagaki 2:983a818c6ddf 336 printf("String = \"%s\" \r\n", Buf_d);
kennakagaki 2:983a818c6ddf 337 }
kennakagaki 2:983a818c6ddf 338 fclose(fp_kd);
kennakagaki 2:983a818c6ddf 339
kennakagaki 2:983a818c6ddf 340 Kd = (float)atof(Buf_d);
kennakagaki 2:983a818c6ddf 341 printf("Kd is loaded as %f. \r\n", Kd);
kennakagaki 2:983a818c6ddf 342
kennakagaki 2:983a818c6ddf 343 //theta_d read
kennakagaki 2:983a818c6ddf 344 FILE *fp_th = fopen("/sd/mydir/param/theta_d.txt", "r");
kennakagaki 2:983a818c6ddf 345 char Buf_t[30];
kennakagaki 2:983a818c6ddf 346 if(fp_th == NULL) { error("Could not open file for reading\r\n"); }
kennakagaki 2:983a818c6ddf 347 while(fgets (Buf_t, 30, fp_th) != NULL){
kennakagaki 2:983a818c6ddf 348 Buf_t[strlen(Buf_t)-1] = 0;
kennakagaki 2:983a818c6ddf 349 printf("String = \"%s\" \r\n", Buf_t);
kennakagaki 2:983a818c6ddf 350 }
kennakagaki 2:983a818c6ddf 351 fclose(fp_th);
kennakagaki 2:983a818c6ddf 352
kennakagaki 2:983a818c6ddf 353 theta_d = (float)atof(Buf_t);
kennakagaki 2:983a818c6ddf 354 printf("theta_d is loaded as %f. \r\n", fp_th);
kennakagaki 2:983a818c6ddf 355
kennakagaki 2:983a818c6ddf 356 }
kennakagaki 2:983a818c6ddf 357
kennakagaki 2:983a818c6ddf 358 void saveParam(){ //to SD
kennakagaki 2:983a818c6ddf 359
kennakagaki 2:983a818c6ddf 360 // -- kp
kennakagaki 2:983a818c6ddf 361 FILE *fp_kp = fopen("/sd/mydir/param/kp.txt", "w");
kennakagaki 2:983a818c6ddf 362 if(fp_kp == NULL) {
kennakagaki 2:983a818c6ddf 363 error("Could not open file for write\n");
kennakagaki 2:983a818c6ddf 364 }
kennakagaki 2:983a818c6ddf 365 char buf_p[10];
kennakagaki 2:983a818c6ddf 366 sprintf(buf_p, "%.3f", Kp);
kennakagaki 2:983a818c6ddf 367 fprintf(fp_kp, buf_p);
kennakagaki 2:983a818c6ddf 368 fprintf(fp_kp, "\r\n");
kennakagaki 2:983a818c6ddf 369 fclose(fp_kp);
kennakagaki 2:983a818c6ddf 370 printf("Kp saved as %f.\r\n", Kp);
kennakagaki 2:983a818c6ddf 371
kennakagaki 2:983a818c6ddf 372 // -- kd
kennakagaki 2:983a818c6ddf 373 FILE *fp_kd = fopen("/sd/mydir/param/kd.txt", "w");
kennakagaki 2:983a818c6ddf 374 if(fp_kd == NULL) {
kennakagaki 2:983a818c6ddf 375 error("Could not open file for write\n");
kennakagaki 2:983a818c6ddf 376 }
kennakagaki 2:983a818c6ddf 377 char buf_d[10];
kennakagaki 2:983a818c6ddf 378 sprintf(buf_d, "%.3f", Kd);
kennakagaki 2:983a818c6ddf 379 fprintf(fp_kd, buf_d);
kennakagaki 2:983a818c6ddf 380 fprintf(fp_kd, "\r\n");
kennakagaki 2:983a818c6ddf 381 fclose(fp_kd);
kennakagaki 2:983a818c6ddf 382 printf("Kd saved as %f.\r\n", Kd);
kennakagaki 2:983a818c6ddf 383
kennakagaki 2:983a818c6ddf 384 // -- theta_d
kennakagaki 2:983a818c6ddf 385 FILE *fp_th = fopen("/sd/mydir/param/theta_d.txt", "w");
kennakagaki 2:983a818c6ddf 386 if(fp_th == NULL) {
kennakagaki 2:983a818c6ddf 387 error("Could not open file for write\n");
kennakagaki 2:983a818c6ddf 388 }
kennakagaki 2:983a818c6ddf 389 char buf_t[10];
kennakagaki 2:983a818c6ddf 390 sprintf(buf_t, "%.3f", theta_d);
kennakagaki 2:983a818c6ddf 391 fprintf(fp_th, buf_t);
kennakagaki 2:983a818c6ddf 392 fprintf(fp_th, "\r\n");
kennakagaki 2:983a818c6ddf 393 fclose(fp_th);
kennakagaki 2:983a818c6ddf 394 printf("theta_d saved as %f.\r\n", theta_d);
kennakagaki 2:983a818c6ddf 395
kennakagaki 2:983a818c6ddf 396 }
kennakagaki 2:983a818c6ddf 397
kennakagaki 2:983a818c6ddf 398 char buffer_serial[20];
kennakagaki 2:983a818c6ddf 399 void serialUpdateVal(){
kennakagaki 2:983a818c6ddf 400
kennakagaki 2:983a818c6ddf 401 pc.scanf("%s", &buffer_serial);
kennakagaki 2:983a818c6ddf 402 pc.printf("I received ");
kennakagaki 2:983a818c6ddf 403 pc.printf(buffer_serial);
kennakagaki 2:983a818c6ddf 404 pc.printf("\n");
kennakagaki 2:983a818c6ddf 405
kennakagaki 2:983a818c6ddf 406 if(inputMode == 'n'){
kennakagaki 2:983a818c6ddf 407 if(buffer_serial[0] == 'p'){
kennakagaki 2:983a818c6ddf 408 pc.printf("input mode is set to Kp, Please enter value. \n");
kennakagaki 2:983a818c6ddf 409 inputMode = 'p';
kennakagaki 2:983a818c6ddf 410 } if(buffer_serial[0] == 'd'){
kennakagaki 2:983a818c6ddf 411 pc.printf("input mode is set to Kd, Please enter value. \n");
kennakagaki 2:983a818c6ddf 412 inputMode = 'd';
kennakagaki 2:983a818c6ddf 413 } if(buffer_serial[0] == 't'){
kennakagaki 2:983a818c6ddf 414 pc.printf("input mode is set to theta_d, Please enter value. \n");
kennakagaki 2:983a818c6ddf 415 inputMode = 't';
kennakagaki 2:983a818c6ddf 416 }
kennakagaki 2:983a818c6ddf 417 } else if(inputMode == 'p'){
kennakagaki 2:983a818c6ddf 418 Kp = (float)atof(buffer_serial);
kennakagaki 2:983a818c6ddf 419 inputMode = 'n';
kennakagaki 2:983a818c6ddf 420 pc.printf("Kp is set to %f \n\n", Kp);
kennakagaki 2:983a818c6ddf 421 saveParam();
kennakagaki 2:983a818c6ddf 422 } else if(inputMode == 'd'){
kennakagaki 2:983a818c6ddf 423 Kd = (float)atof(buffer_serial);
kennakagaki 2:983a818c6ddf 424 inputMode = 'n';
kennakagaki 2:983a818c6ddf 425 pc.printf("Kd is set to %f \n\n", Kd);
kennakagaki 2:983a818c6ddf 426 saveParam();
kennakagaki 2:983a818c6ddf 427 } else if(inputMode == 't'){
kennakagaki 2:983a818c6ddf 428 theta_d = (float)atof(buffer_serial);
kennakagaki 2:983a818c6ddf 429 inputMode = 'n';
kennakagaki 2:983a818c6ddf 430 pc.printf("theta_d is set to %f \n\n", theta_d);
kennakagaki 2:983a818c6ddf 431 saveParam();
kennakagaki 2:983a818c6ddf 432 }
kennakagaki 2:983a818c6ddf 433
kennakagaki 2:983a818c6ddf 434
kennakagaki 2:983a818c6ddf 435 if(buffer_serial[0] == 'c'){
kennakagaki 2:983a818c6ddf 436 pc.printf("Kp is %f | Kd is %f | theta_d is %f \n\n", Kp, Kd, theta_d);
kennakagaki 2:983a818c6ddf 437 }
kennakagaki 2:983a818c6ddf 438
kennakagaki 2:983a818c6ddf 439 }
kennakagaki 2:983a818c6ddf 440
kennakagaki 2:983a818c6ddf 441 /******************** LogData to SD card *****************************/
kennakagaki 2:983a818c6ddf 442
kennakagaki 2:983a818c6ddf 443 int trialTimeCount = 0;
kennakagaki 2:983a818c6ddf 444
kennakagaki 2:983a818c6ddf 445 const int logValNum = 4;
kennakagaki 2:983a818c6ddf 446 char logValName[logValNum][30];
kennakagaki 2:983a818c6ddf 447 float logVal[logValNum][10000];
kennakagaki 2:983a818c6ddf 448 int currentLogNum = 0;
kennakagaki 2:983a818c6ddf 449
kennakagaki 2:983a818c6ddf 450 void updateTrialTime(){
kennakagaki 2:983a818c6ddf 451 //trial time read
kennakagaki 2:983a818c6ddf 452 FILE *fp_tr = fopen("/sd/mydir/trialtime.txt", "r");
kennakagaki 2:983a818c6ddf 453
kennakagaki 2:983a818c6ddf 454 char Buffer_t[512];
kennakagaki 2:983a818c6ddf 455
kennakagaki 2:983a818c6ddf 456 if(fp_tr == NULL) { error("Could not open file for reading\r\n"); }
kennakagaki 2:983a818c6ddf 457
kennakagaki 2:983a818c6ddf 458 while(fgets (Buffer_t, 512, fp_tr) != NULL){
kennakagaki 2:983a818c6ddf 459 Buffer_t[strlen(Buffer_t)-1] = 0;
kennakagaki 2:983a818c6ddf 460 printf("String = \"%s\" \r\n", Buffer_t);
kennakagaki 2:983a818c6ddf 461 }
kennakagaki 2:983a818c6ddf 462 fclose(fp_tr);
kennakagaki 2:983a818c6ddf 463
kennakagaki 2:983a818c6ddf 464 trialTimeCount = (int)atof(Buffer_t);
kennakagaki 2:983a818c6ddf 465
kennakagaki 2:983a818c6ddf 466 printf("last trialTimeCount was %i. \n", trialTimeCount);
kennakagaki 2:983a818c6ddf 467
kennakagaki 2:983a818c6ddf 468 trialTimeCount++; //count up trial time
kennakagaki 2:983a818c6ddf 469 printf("current trialTimeCount updated to %i. \n", trialTimeCount);
kennakagaki 2:983a818c6ddf 470
kennakagaki 2:983a818c6ddf 471 FILE *fp3 = fopen("/sd/mydir/trialtime.txt", "w");
kennakagaki 2:983a818c6ddf 472 if(fp3 == NULL) {
kennakagaki 2:983a818c6ddf 473 error("Could not open file for write\n");
kennakagaki 2:983a818c6ddf 474 }
kennakagaki 2:983a818c6ddf 475 char buf[10];
kennakagaki 2:983a818c6ddf 476
kennakagaki 2:983a818c6ddf 477 sprintf(buf, "%d", trialTimeCount);
kennakagaki 2:983a818c6ddf 478
kennakagaki 2:983a818c6ddf 479 fprintf(fp3, buf);
kennakagaki 2:983a818c6ddf 480 fprintf(fp3, "\r\n");
kennakagaki 2:983a818c6ddf 481 fclose(fp3);
kennakagaki 2:983a818c6ddf 482 printf("trial time saved\n");
kennakagaki 2:983a818c6ddf 483 }
kennakagaki 2:983a818c6ddf 484
kennakagaki 2:983a818c6ddf 485 void logData(){ // log data time
kennakagaki 2:983a818c6ddf 486 logVal[0][currentLogNum] = t.read();
kennakagaki 2:983a818c6ddf 487 logVal[1][currentLogNum] = accX;
kennakagaki 2:983a818c6ddf 488 logVal[2][currentLogNum] = accY;
kennakagaki 2:983a818c6ddf 489 logVal[3][currentLogNum] = accZ;
kennakagaki 2:983a818c6ddf 490
kennakagaki 2:983a818c6ddf 491 printf("logged data for %i. t.read() = %f \r\n", currentLogNum, t.read());
kennakagaki 2:983a818c6ddf 492
kennakagaki 2:983a818c6ddf 493 currentLogNum++;
kennakagaki 2:983a818c6ddf 494 }
kennakagaki 2:983a818c6ddf 495
kennakagaki 2:983a818c6ddf 496 void saveData(){ // call when the while loop ends or the button pressed
kennakagaki 2:983a818c6ddf 497 sprintf( logValName[0], "time");
kennakagaki 2:983a818c6ddf 498 sprintf( logValName[1], "accX");
kennakagaki 2:983a818c6ddf 499 sprintf( logValName[2], "accY");
kennakagaki 2:983a818c6ddf 500 sprintf( logValName[3], "accZ");
kennakagaki 2:983a818c6ddf 501
kennakagaki 2:983a818c6ddf 502 char filename[256];
kennakagaki 2:983a818c6ddf 503 sprintf(filename, "/sd/mydir/log/flipper_logData_%i.txt",trialTimeCount);
kennakagaki 2:983a818c6ddf 504
kennakagaki 2:983a818c6ddf 505 FILE *f_sv = fopen(filename, "w");
kennakagaki 2:983a818c6ddf 506 if(f_sv == NULL) {
kennakagaki 2:983a818c6ddf 507 error("Could not open file for write\n");
kennakagaki 2:983a818c6ddf 508 }
kennakagaki 2:983a818c6ddf 509
kennakagaki 2:983a818c6ddf 510 for(int i = 0; i < logValNum; i++){
kennakagaki 2:983a818c6ddf 511 fprintf(f_sv, logValName[i]);
kennakagaki 2:983a818c6ddf 512 fprintf(f_sv, ",");
kennakagaki 2:983a818c6ddf 513 }
kennakagaki 2:983a818c6ddf 514 fprintf(f_sv, "\r\n");
kennakagaki 2:983a818c6ddf 515
kennakagaki 2:983a818c6ddf 516 for(int j = 0; j < currentLogNum; j++){
kennakagaki 2:983a818c6ddf 517 for(int i = 0; i < logValNum; i++){
kennakagaki 2:983a818c6ddf 518 //char buf_temp[10];
kennakagaki 2:983a818c6ddf 519 //int a = 5;
kennakagaki 2:983a818c6ddf 520
kennakagaki 2:983a818c6ddf 521 char cVal[8];
kennakagaki 2:983a818c6ddf 522
kennakagaki 2:983a818c6ddf 523 sprintf(cVal,"%.3f", logVal[i][j]);
kennakagaki 2:983a818c6ddf 524
kennakagaki 2:983a818c6ddf 525 //sprintf(buf_temp, "%f", logVal[i][j]);
kennakagaki 2:983a818c6ddf 526
kennakagaki 2:983a818c6ddf 527 fprintf(f_sv, cVal);
kennakagaki 2:983a818c6ddf 528 fprintf(f_sv, ",");
kennakagaki 2:983a818c6ddf 529 }
kennakagaki 2:983a818c6ddf 530 fprintf(f_sv, "\r\n");
kennakagaki 2:983a818c6ddf 531 }
kennakagaki 2:983a818c6ddf 532
kennakagaki 2:983a818c6ddf 533
kennakagaki 2:983a818c6ddf 534 fclose(f_sv);
kennakagaki 2:983a818c6ddf 535
kennakagaki 2:983a818c6ddf 536 printf("Log Data file is saved as 'flipper_logData_%i.txt'.\n", trialTimeCount);
kennakagaki 2:983a818c6ddf 537 }
kennakagaki 2:983a818c6ddf 538
DVLevine 1:59124c69d0c3 539 /*******************************************************************/
DVLevine 1:59124c69d0c3 540
kennakagaki 2:983a818c6ddf 541
kennakagaki 2:983a818c6ddf 542
DVLevine 1:59124c69d0c3 543 /******************** EXECUTE MOTOR LOOP *****************************/
DVLevine 1:59124c69d0c3 544
DVLevine 1:59124c69d0c3 545 float pikeAngle = 0.0;
DVLevine 1:59124c69d0c3 546 float hitTimes = 0;
DVLevine 1:59124c69d0c3 547
DVLevine 1:59124c69d0c3 548 /** Core Motor Code Loop**/
DVLevine 1:59124c69d0c3 549 void executeMotorLoop(){
DVLevine 1:59124c69d0c3 550
DVLevine 1:59124c69d0c3 551 printf("Entering Motor Loop\n");
DVLevine 1:59124c69d0c3 552 // initial setup
DVLevine 1:59124c69d0c3 553 t.reset();
DVLevine 1:59124c69d0c3 554 t.start();
DVLevine 1:59124c69d0c3 555 encoder.reset();
DVLevine 1:59124c69d0c3 556 setMotorDir('f'); //begin with motor driving forward
DVLevine 1:59124c69d0c3 557 motorPWM.write(0);
DVLevine 1:59124c69d0c3 558 lastTime=t.read();
DVLevine 0:73a369b7b5b4 559
DVLevine 1:59124c69d0c3 560 float timeSinceCollision=0.0;
DVLevine 1:59124c69d0c3 561
DVLevine 1:59124c69d0c3 562 // Run experiment
kennakagaki 2:983a818c6ddf 563 while( t.read() < 10.0 && !debugModeOn) {
DVLevine 1:59124c69d0c3 564 //Update IMU and Kalman
DVLevine 1:59124c69d0c3 565 readIMU();
DVLevine 1:59124c69d0c3 566
DVLevine 1:59124c69d0c3 567 //R = sqrt(std::pow(accX, 2) + std::pow(accY, 2) + std::pow(accZ, 2));
DVLevine 1:59124c69d0c3 568 //getKalmanPrediction(t.read()-lastTime, gyroY, accY, R);
DVLevine 1:59124c69d0c3 569 //printf("kalman angle: %f\n",kalman_get_angle(&filterRotation));
DVLevine 1:59124c69d0c3 570
DVLevine 1:59124c69d0c3 571 //read present time and calculate dt
DVLevine 1:59124c69d0c3 572 presentTime = t.read();
DVLevine 1:59124c69d0c3 573 dT = presentTime - lastTime;
DVLevine 1:59124c69d0c3 574
DVLevine 1:59124c69d0c3 575 float floatme = (accX-last_xAcc)/dT;
DVLevine 1:59124c69d0c3 576
DVLevine 1:59124c69d0c3 577 //printf("last %f\n",last_xAcc);
DVLevine 1:59124c69d0c3 578 //printf("now %f\n", accX);
DVLevine 1:59124c69d0c3 579 //printf("%f\n",floatme);
DVLevine 1:59124c69d0c3 580 lastTime = presentTime;
DVLevine 1:59124c69d0c3 581
DVLevine 1:59124c69d0c3 582 // Perform control loop logic
DVLevine 1:59124c69d0c3 583 theta = getAngleFromPulses(encoder.getPulses());
DVLevine 1:59124c69d0c3 584 theta_dot = getAngleFromPulses(encoder.getVelocity());
DVLevine 1:59124c69d0c3 585
DVLevine 1:59124c69d0c3 586 //check collisions
DVLevine 1:59124c69d0c3 587
DVLevine 1:59124c69d0c3 588 //If new hit. Otherwise bool_collision will do the work.
DVLevine 1:59124c69d0c3 589 if ((!bool_collision) & checkCollision(accX,dT)){
DVLevine 1:59124c69d0c3 590 collisionTime = t.read();
DVLevine 1:59124c69d0c3 591 collisionTimeRemaining = collisionDuration;
DVLevine 1:59124c69d0c3 592 bool_collision = true;
DVLevine 1:59124c69d0c3 593 hitTimes = hitTimes + 1;
DVLevine 1:59124c69d0c3 594 }else{
DVLevine 1:59124c69d0c3 595 //see if actuation is maintained:
DVLevine 1:59124c69d0c3 596 bool_collision = checkCollision(accX,dT);
DVLevine 1:59124c69d0c3 597 }
DVLevine 1:59124c69d0c3 598
DVLevine 1:59124c69d0c3 599 //originally default state (drop)
DVLevine 1:59124c69d0c3 600 //torque is defaulty 0
DVLevine 1:59124c69d0c3 601 torque = 0.0;
DVLevine 1:59124c69d0c3 602
DVLevine 1:59124c69d0c3 603 // state when in stance (bool collision)
DVLevine 1:59124c69d0c3 604 if (bool_collision){
DVLevine 1:59124c69d0c3 605 torque = -0.2;
DVLevine 1:59124c69d0c3 606 i_d = torque/k_b;
DVLevine 1:59124c69d0c3 607
DVLevine 1:59124c69d0c3 608 //Update Error and accumalated error term
DVLevine 1:59124c69d0c3 609 i = readCurrent();
DVLevine 1:59124c69d0c3 610 e = i_d-i;
DVLevine 1:59124c69d0c3 611 outputVal = r*i_d+Kp*(e)-k_b*theta_dot;
DVLevine 1:59124c69d0c3 612 timeSinceCollision = t.read();
DVLevine 1:59124c69d0c3 613 }
DVLevine 1:59124c69d0c3 614
DVLevine 1:59124c69d0c3 615 // state in the air (after bool collision duration off)
DVLevine 1:59124c69d0c3 616 else{
DVLevine 1:59124c69d0c3 617
DVLevine 1:59124c69d0c3 618 if (hitTimes >1){
DVLevine 1:59124c69d0c3 619 if ((t.read()-timeSinceCollision)<0.3){
DVLevine 1:59124c69d0c3 620 theta_d = -0.3;
DVLevine 1:59124c69d0c3 621 }
DVLevine 1:59124c69d0c3 622 if ( ((t.read()-timeSinceCollision)>0.3) && ((t.read()-timeSinceCollision)<0.7) ){
DVLevine 1:59124c69d0c3 623 theta_d = 0.31;
DVLevine 1:59124c69d0c3 624 }
DVLevine 1:59124c69d0c3 625 if ((t.read()-timeSinceCollision)>=1.0){
DVLevine 1:59124c69d0c3 626 theta_d = 0.64;
DVLevine 1:59124c69d0c3 627 }
DVLevine 1:59124c69d0c3 628
DVLevine 1:59124c69d0c3 629 //at hit -1.7 t=-.3
DVLevine 1:59124c69d0c3 630 //peak -0.7 t=.31
DVLevine 1:59124c69d0c3 631 //end is 0.2 t=.64
DVLevine 1:59124c69d0c3 632 e = theta_d-theta;
DVLevine 1:59124c69d0c3 633 //e_accum = e_accum+e*dT;
DVLevine 1:59124c69d0c3 634 Kp = 5;
DVLevine 1:59124c69d0c3 635 Kd = 0.5;
DVLevine 1:59124c69d0c3 636 outputVal = Kp*e + Kd*(-theta_dot);
DVLevine 1:59124c69d0c3 637 }
DVLevine 1:59124c69d0c3 638 }
DVLevine 1:59124c69d0c3 639
DVLevine 1:59124c69d0c3 640 if (hitTimes<=1){
DVLevine 1:59124c69d0c3 641 outputVal = 0;
DVLevine 1:59124c69d0c3 642 }
kennakagaki 2:983a818c6ddf 643 // printf("%f",hitTimes);
DVLevine 1:59124c69d0c3 644
DVLevine 1:59124c69d0c3 645
DVLevine 1:59124c69d0c3 646
DVLevine 1:59124c69d0c3 647
DVLevine 1:59124c69d0c3 648
DVLevine 1:59124c69d0c3 649 //direct input for torque
DVLevine 1:59124c69d0c3 650 /* if (bool_collision){
DVLevine 1:59124c69d0c3 651 torque = 1.4;
DVLevine 1:59124c69d0c3 652 }else{
DVLevine 1:59124c69d0c3 653 torque = 0.0;
DVLevine 1:59124c69d0c3 654 }*/
DVLevine 1:59124c69d0c3 655 last_xAcc = accX;
DVLevine 1:59124c69d0c3 656 //printf("HEY\n");
DVLevine 1:59124c69d0c3 657
DVLevine 1:59124c69d0c3 658 //set reference current for torque
DVLevine 1:59124c69d0c3 659 //theta_d = 0.0;//M_PI; // target at 180 degrees;
DVLevine 1:59124c69d0c3 660 //torque = -k_th*(theta_d-theta)-b_th*-theta_dot+v_th*-theta_dot;
DVLevine 1:59124c69d0c3 661
DVLevine 1:59124c69d0c3 662
DVLevine 1:59124c69d0c3 663 // printf("currentError: %f\n",e);
DVLevine 1:59124c69d0c3 664 //temp removal of force for non collision
DVLevine 1:59124c69d0c3 665 //printf("%f\n",ain.read());
DVLevine 1:59124c69d0c3 666
DVLevine 1:59124c69d0c3 667
DVLevine 1:59124c69d0c3 668 //regular pid
DVLevine 1:59124c69d0c3 669 //Update Error and accumalated error term
DVLevine 1:59124c69d0c3 670 /*e = theta_d-theta;
DVLevine 1:59124c69d0c3 671 e_accum = e_accum+e*dT;
DVLevine 1:59124c69d0c3 672 outputVal = Kp*e + Kd*(-theta_dot) + Ki*e_accum;*/
DVLevine 1:59124c69d0c3 673
DVLevine 1:59124c69d0c3 674 if (outputVal<0){
DVLevine 1:59124c69d0c3 675 //negative difference, need to move motor backward to correct
DVLevine 1:59124c69d0c3 676 setMotorDir('f');
DVLevine 1:59124c69d0c3 677 }
DVLevine 1:59124c69d0c3 678 if (outputVal>0){
DVLevine 1:59124c69d0c3 679 //positive difference, need to move motor forward to correct
DVLevine 1:59124c69d0c3 680 setMotorDir('r');
DVLevine 1:59124c69d0c3 681 }
DVLevine 1:59124c69d0c3 682 motorPWM.write(toDutyCycle(abs(outputVal)));
DVLevine 1:59124c69d0c3 683 //printf("Serial: %f\n",outputVal);
kennakagaki 2:983a818c6ddf 684
kennakagaki 2:983a818c6ddf 685 logData();
kennakagaki 2:983a818c6ddf 686 if(!saveDataButton){
kennakagaki 2:983a818c6ddf 687 saveData();
kennakagaki 2:983a818c6ddf 688 debugModeOn = true;
kennakagaki 2:983a818c6ddf 689 }
kennakagaki 2:983a818c6ddf 690
DVLevine 1:59124c69d0c3 691 }
DVLevine 1:59124c69d0c3 692 motorPWM.write(0);
DVLevine 0:73a369b7b5b4 693 }
DVLevine 1:59124c69d0c3 694
kennakagaki 2:983a818c6ddf 695
kennakagaki 2:983a818c6ddf 696
kennakagaki 2:983a818c6ddf 697
DVLevine 1:59124c69d0c3 698 /****************************** MAIN *************************************/
DVLevine 1:59124c69d0c3 699
DVLevine 1:59124c69d0c3 700 int main()
DVLevine 1:59124c69d0c3 701 {
kennakagaki 2:983a818c6ddf 702
kennakagaki 2:983a818c6ddf 703 printf("test!!\n");
kennakagaki 2:983a818c6ddf 704 updateTrialTime();
kennakagaki 2:983a818c6ddf 705 loadParam();
kennakagaki 2:983a818c6ddf 706
DVLevine 1:59124c69d0c3 707 /* setup imu and kalman filter for rotation */
DVLevine 1:59124c69d0c3 708 setupIMU();
DVLevine 1:59124c69d0c3 709 setupKalman();
DVLevine 1:59124c69d0c3 710
kennakagaki 2:983a818c6ddf 711 //while( t.read() < 60.0) {
DVLevine 1:59124c69d0c3 712 /* run core program */
DVLevine 1:59124c69d0c3 713 executeMotorLoop();
kennakagaki 2:983a818c6ddf 714
kennakagaki 2:983a818c6ddf 715
kennakagaki 2:983a818c6ddf 716 //}
kennakagaki 2:983a818c6ddf 717 saveData();
kennakagaki 2:983a818c6ddf 718
kennakagaki 2:983a818c6ddf 719 printf("Now you can set parameters by typing 'p', 'd', or 't'. You can check the values with 'c'.");
kennakagaki 2:983a818c6ddf 720 while(1){ // debugMode
kennakagaki 2:983a818c6ddf 721 serialUpdateVal();
kennakagaki 2:983a818c6ddf 722 }
DVLevine 1:59124c69d0c3 723
DVLevine 1:59124c69d0c3 724 /* extra code for threading accelerometer kalman data */
DVLevine 1:59124c69d0c3 725 //Thread eventThread;
DVLevine 1:59124c69d0c3 726 //eventQueue.call_every(0.00001, periodicIMUCallback);
DVLevine 1:59124c69d0c3 727 //eventQueue.dispatch_forever();
DVLevine 1:59124c69d0c3 728 //eventThread.start(callback(&eventQueue, &EventQueue::dispatch_forever));
DVLevine 1:59124c69d0c3 729
DVLevine 1:59124c69d0c3 730
DVLevine 1:59124c69d0c3 731 /*eventQueue.call_every(0.2, periodicCallback);
DVLevine 1:59124c69d0c3 732
DVLevine 1:59124c69d0c3 733 BLE &ble = BLE::Instance();
DVLevine 1:59124c69d0c3 734 ble.onEventsToProcess(scheduleBleEventsProcessing);
DVLevine 1:59124c69d0c3 735 ble.init(bleInitComplete);
DVLevine 1:59124c69d0c3 736 /*eventQueue.call_every(0.2, periodicCallback);
DVLevine 1:59124c69d0c3 737 eventQueue.dispatch_forever();
DVLevine 1:59124c69d0c3 738
DVLevine 1:59124c69d0c3 739 return 0;*/
DVLevine 1:59124c69d0c3 740 }