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@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 }