MRD Lab / SpindleBot_1_5b

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:   MX12 ServoRingBuffer mbed-src

Fork of SpindleBot by MRD Lab

main.cpp@2:dfeadd6c651c, 2015-01-26 (annotated)

Committer:
labmrd
Date:
Mon Jan 26 04:38:06 2015 +0000
Revision:
2:dfeadd6c651c
Parent:
0:8a555873b7d3
Child:
4:e44ac08027bd
F1rst P0st!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
labmrd 2:dfeadd6c651c 1 #define USE_DYNAMIXELS
labmrd 2:dfeadd6c651c 2 //#define USE_BLUETOOTH
labmrd 2:dfeadd6c651c 3 #define USE_SD_CARD
labmrd 2:dfeadd6c651c 4
labmrd 2:dfeadd6c651c 5 // We have different modes for different things
labmrd 2:dfeadd6c651c 6 #define MODE_MANUAL 1
labmrd 2:dfeadd6c651c 7 #define MODE_AUTOMATIC 2
labmrd 2:dfeadd6c651c 8 #define MODE_IDLE 3
labmrd 2:dfeadd6c651c 9 #define MODE_NULL 0
labmrd 2:dfeadd6c651c 10
labmrd 2:dfeadd6c651c 11 // We always want to know if we are closing or opening
labmrd 2:dfeadd6c651c 12 #define DIRECTION_CLOSING 1
labmrd 2:dfeadd6c651c 13 #define DIRECTION_OPENING 2
labmrd 2:dfeadd6c651c 14 #define DIRECTION_SLACK_WATER 3
labmrd 2:dfeadd6c651c 15 #define DIRECTION_NULL 0
labmrd 2:dfeadd6c651c 16
labmrd 2:dfeadd6c651c 17 // General includes
mbed_official 0:8a555873b7d3 18 #include "mbed.h"
labmrd 2:dfeadd6c651c 19 #include "ServoRingBuffer.h"
labmrd 2:dfeadd6c651c 20 #include "ram_test.h"
labmrd 2:dfeadd6c651c 21 #include "Serial_Receive.h"
labmrd 2:dfeadd6c651c 22 #include <string>
labmrd 2:dfeadd6c651c 23
labmrd 2:dfeadd6c651c 24 // Specific to Dynamixels
labmrd 2:dfeadd6c651c 25 #ifdef USE_DYNAMIXELS
labmrd 2:dfeadd6c651c 26 #include "MX12.h"
labmrd 2:dfeadd6c651c 27 #endif
labmrd 2:dfeadd6c651c 28
labmrd 2:dfeadd6c651c 29 // Specific to SD Card
labmrd 2:dfeadd6c651c 30 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 31 #include "SDFileSystem.h"
labmrd 2:dfeadd6c651c 32 #endif
labmrd 2:dfeadd6c651c 33
labmrd 2:dfeadd6c651c 34 // Everyone should know pi...
labmrd 2:dfeadd6c651c 35 #ifndef M_PI
labmrd 2:dfeadd6c651c 36 #define M_PI 3.14159265358979323846 /* pi */
labmrd 2:dfeadd6c651c 37 #endif
labmrd 2:dfeadd6c651c 38 #ifndef M_PI_2
labmrd 2:dfeadd6c651c 39 #define M_PI_2 1.57079632679489661923 /* pi/2 */
labmrd 2:dfeadd6c651c 40 #endif
labmrd 2:dfeadd6c651c 41
labmrd 2:dfeadd6c651c 42
labmrd 2:dfeadd6c651c 43
labmrd 2:dfeadd6c651c 44 // Define pins and interrupts
labmrd 2:dfeadd6c651c 45 Ticker potISR; //Define a recurring timer interrupt
labmrd 2:dfeadd6c651c 46 DigitalOut led1(LED1); //Led 1 for debugging purposes
labmrd 2:dfeadd6c651c 47 DigitalOut led2(LED2); //Led 2 for debugging purposes
labmrd 2:dfeadd6c651c 48 DigitalOut led3(LED3); //Led 3 for debugging purposes
labmrd 2:dfeadd6c651c 49 DigitalOut led4(LED4); //Led 4 for debugging purposes
labmrd 2:dfeadd6c651c 50 DigitalOut triggerOut(p11);
labmrd 2:dfeadd6c651c 51 Serial pc(USBTX, USBRX); //Set up serial connection to pc
labmrd 2:dfeadd6c651c 52 #ifdef USE_BLUETOOTH
labmrd 2:dfeadd6c651c 53 Serial bt(p13,p14); //Set up serial connection to bluetooth adapter
labmrd 2:dfeadd6c651c 54 #endif
labmrd 2:dfeadd6c651c 55
labmrd 2:dfeadd6c651c 56 AnalogIn AinLeftForce(p16); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 57 AnalogIn AinRightForce(p15); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 58
labmrd 2:dfeadd6c651c 59 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 60 // Attach SD card
labmrd 2:dfeadd6c651c 61 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
labmrd 2:dfeadd6c651c 62 FILE *fp = NULL;
labmrd 2:dfeadd6c651c 63 #define SAMPLES_PER_FILE 10000
labmrd 2:dfeadd6c651c 64 #endif
labmrd 2:dfeadd6c651c 65
labmrd 2:dfeadd6c651c 66 // Dummy variable for debugging
labmrd 2:dfeadd6c651c 67 unsigned int global_count=0;
labmrd 2:dfeadd6c651c 68 float max_percent_full=0;
labmrd 2:dfeadd6c651c 69
labmrd 2:dfeadd6c651c 70 // Define variables for the program
labmrd 2:dfeadd6c651c 71 float servoAngle; //This is the desired servo angle based on the scaled potentiometer value
labmrd 2:dfeadd6c651c 72 float potData; //This is the value of the potentiometer from Ain.read()
labmrd 2:dfeadd6c651c 73 bool collect_data = false; //This is
labmrd 2:dfeadd6c651c 74
labmrd 2:dfeadd6c651c 75 bool keyStrokeFlag = false; //This is a flag to see if a keystroke has been pressed
labmrd 2:dfeadd6c651c 76 char keyStrokeVal; //This is a character storing the value of the keystroke
labmrd 2:dfeadd6c651c 77
labmrd 2:dfeadd6c651c 78 char g_tissue_type_name[32];
labmrd 2:dfeadd6c651c 79 float g_frequency;
labmrd 2:dfeadd6c651c 80 int g_max_force;
labmrd 2:dfeadd6c651c 81 int g_num_cycles;
labmrd 2:dfeadd6c651c 82 float g_current_trajectory_time;
labmrd 2:dfeadd6c651c 83 float g_theta;
labmrd 2:dfeadd6c651c 84 float g_theta_last=0;
labmrd 2:dfeadd6c651c 85 unsigned char g_current_mode=MODE_NULL;
labmrd 2:dfeadd6c651c 86 unsigned char g_current_direction=DIRECTION_NULL;
labmrd 2:dfeadd6c651c 87 unsigned char g_current_cycle=0;
labmrd 2:dfeadd6c651c 88
labmrd 2:dfeadd6c651c 89 // Warning, this buffer is large!
labmrd 2:dfeadd6c651c 90 ServoRingBuffer Buffer;
labmrd 2:dfeadd6c651c 91 spindleData tempSpindleData; //For sending to the buffer
labmrd 2:dfeadd6c651c 92
labmrd 2:dfeadd6c651c 93 Timer ISRDurationTimer;
labmrd 2:dfeadd6c651c 94 Timer AuxSerialTimer;
labmrd 2:dfeadd6c651c 95 int worst_latency=0;
labmrd 2:dfeadd6c651c 96 int current_latency;
labmrd 2:dfeadd6c651c 97
labmrd 2:dfeadd6c651c 98 #ifdef USE_DYNAMIXELS
labmrd 2:dfeadd6c651c 99 //Dynamixels can only handle 500Hz for now. Working on it...
labmrd 2:dfeadd6c651c 100 float samplingPeriod = 0.002; //This is the sampling period for the timer interrupt
labmrd 2:dfeadd6c651c 101 #define LEFT_JAW_DYNAMIXEL_ID 3
labmrd 2:dfeadd6c651c 102 #define RIGHT_JAW_DYNAMIXEL_ID 4
labmrd 2:dfeadd6c651c 103 #define MIN_SERVO_ANGLE_Da_VINCI 1600 //This is the open in encoder counts
labmrd 2:dfeadd6c651c 104 #define MAX_SERVO_ANGLE_Da_VINCI 3200 //This is the closed in encoder counts
labmrd 2:dfeadd6c651c 105 // Dynamixel Object
labmrd 2:dfeadd6c651c 106 MX12 mx12_left_jaw (p28, p27, LEFT_JAW_DYNAMIXEL_ID, 1000000);
labmrd 2:dfeadd6c651c 107 MX12 mx12_right_jaw (p28, p27, RIGHT_JAW_DYNAMIXEL_ID, 1000000);
labmrd 2:dfeadd6c651c 108
labmrd 2:dfeadd6c651c 109 /// Set these to outputs so that they don't interfere with the serial communication
labmrd 2:dfeadd6c651c 110 DigitalIn nullOut1(p21);
labmrd 2:dfeadd6c651c 111 DigitalIn nullOut2(p22);
labmrd 2:dfeadd6c651c 112 DigitalIn nullOut3(p23);
labmrd 2:dfeadd6c651c 113 DigitalIn nullOut4(p24);
labmrd 2:dfeadd6c651c 114 #else
labmrd 2:dfeadd6c651c 115 float samplingPeriod = 0.001; //This is the sampling period for the timer interrupt
labmrd 2:dfeadd6c651c 116 #define SERVO_DEGREE_0 900 //This is the pulse width value for HiTEC-422 in microseconds to turn 0 degrees
labmrd 2:dfeadd6c651c 117 #define SERVO_DEGREE_180 2100 //This is the pulse width value for HiTEC-422 in microseconds to turn 180 degrees
labmrd 2:dfeadd6c651c 118 #define MIN_SERVO_ANGLE 0.0 //This is the minimum servo angle in degrees
labmrd 2:dfeadd6c651c 119 #define MAX_SERVO_ANGLE 180.0 //This is the maximum servo angle in degrees
labmrd 2:dfeadd6c651c 120 #define MIN_SERVO_ANGLE_Da_VINCI 20.0 //This is the minimum servo angle in degrees
labmrd 2:dfeadd6c651c 121 #define MAX_SERVO_ANGLE_Da_VINCI 100.0 //This is the maximum servo angle in degrees
labmrd 2:dfeadd6c651c 122 const float servoConversion = ((SERVO_DEGREE_180-SERVO_DEGREE_0)/(MAX_SERVO_ANGLE - MIN_SERVO_ANGLE))/1000000.0; //This is the interpolation between min and max servo values
labmrd 2:dfeadd6c651c 123 const float servoOffset = SERVO_DEGREE_0/1000000.0; //This is the pulsewidth value (in seconds) that corresponds to 0 degrees (i.e.-the offset)
labmrd 2:dfeadd6c651c 124
labmrd 2:dfeadd6c651c 125 PwmOut myServoLeft(p21); //Set up servo on pin 21
labmrd 2:dfeadd6c651c 126 PwmOut myServoRight(p22); //Set up servo on pin 22
labmrd 2:dfeadd6c651c 127 AnalogIn AinLeftPosition(p20); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 128 AnalogIn AinRightPosition(p19); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 129
labmrd 2:dfeadd6c651c 130
labmrd 2:dfeadd6c651c 131 // Function moveServoTo: Convert a degree value to pulsewidth for Servo
labmrd 2:dfeadd6c651c 132 void moveServoTo(float angle) {
labmrd 2:dfeadd6c651c 133 // Make sure none of the user input falls outside of min and max angle limits
labmrd 2:dfeadd6c651c 134 if( angle < MIN_SERVO_ANGLE){angle = MIN_SERVO_ANGLE;}
labmrd 2:dfeadd6c651c 135 else if(angle > MAX_SERVO_ANGLE){angle = MAX_SERVO_ANGLE;}
labmrd 2:dfeadd6c651c 136 myServoLeft.pulsewidth(servoOffset + servoConversion*(180-angle));
labmrd 2:dfeadd6c651c 137 myServoRight.pulsewidth(servoOffset + servoConversion*(angle));
labmrd 2:dfeadd6c651c 138 }
labmrd 2:dfeadd6c651c 139
labmrd 2:dfeadd6c651c 140 #endif
labmrd 2:dfeadd6c651c 141
labmrd 2:dfeadd6c651c 142 // Function timerISRFunction: Timer ISR function to collect data and write to ring buffer
labmrd 2:dfeadd6c651c 143 void timerISRFunction() {
labmrd 2:dfeadd6c651c 144 if(collect_data){
labmrd 2:dfeadd6c651c 145 //led 1 is used as a 'thinking' light, brighter=worse
labmrd 2:dfeadd6c651c 146 led1 = 1;
labmrd 2:dfeadd6c651c 147 led2 = 0;
labmrd 2:dfeadd6c651c 148 triggerOut = 1;
labmrd 2:dfeadd6c651c 149
labmrd 2:dfeadd6c651c 150 ISRDurationTimer.reset();
labmrd 2:dfeadd6c651c 151 ISRDurationTimer.start();
labmrd 2:dfeadd6c651c 152
labmrd 2:dfeadd6c651c 153 // Warning, this calculation is in the ISR and as such is probably slower than we would prefer.
labmrd 2:dfeadd6c651c 154 // @todo The math could certainly be optimized with some precalculated constants. Lookup tables are faster than sin()
labmrd 2:dfeadd6c651c 155 float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
labmrd 2:dfeadd6c651c 156 g_theta=(sin(angle)+1)/2.0*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI;
labmrd 2:dfeadd6c651c 157 g_current_direction=(cos(angle)<0);
labmrd 2:dfeadd6c651c 158 g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
labmrd 2:dfeadd6c651c 159
labmrd 2:dfeadd6c651c 160 #ifdef USE_DYNAMIXELS
labmrd 2:dfeadd6c651c 161 tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = AinLeftForce.read_u16();
labmrd 2:dfeadd6c651c 162 tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = mx12_left_jaw.GetRawPosition();
labmrd 2:dfeadd6c651c 163 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
labmrd 2:dfeadd6c651c 164 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = mx12_right_jaw.GetRawPosition();
labmrd 2:dfeadd6c651c 165 tempSpindleData.direction=g_current_direction;
labmrd 2:dfeadd6c651c 166 tempSpindleData.cycle=g_current_cycle;
labmrd 2:dfeadd6c651c 167 Buffer.write(tempSpindleData);
labmrd 2:dfeadd6c651c 168 //mx12_right_jaw.write_short(MX12_REG_GOAL_POSITION,short(g_theta));
labmrd 2:dfeadd6c651c 169 //mx12_right_jaw.coordinated_move(LEFT_JAW_DYNAMIXEL_ID,(short)g_theta, 175, RIGHT_JAW_DYNAMIXEL_ID,(short)g_theta, 175);
labmrd 2:dfeadd6c651c 170 g_current_trajectory_time+=samplingPeriod;
labmrd 2:dfeadd6c651c 171 #else
labmrd 2:dfeadd6c651c 172 tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = AinLeftForce.read_u16();
labmrd 2:dfeadd6c651c 173 tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = AinLeftPosition.read_u16();
labmrd 2:dfeadd6c651c 174 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
labmrd 2:dfeadd6c651c 175 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = AinRightPosition.read_u16();
labmrd 2:dfeadd6c651c 176 tempSpindleData.direction=g_current_direction;
labmrd 2:dfeadd6c651c 177 tempSpindleData.cycle=g_current_cycle;
labmrd 2:dfeadd6c651c 178 Buffer.write(tempSpindleData);
labmrd 2:dfeadd6c651c 179
labmrd 2:dfeadd6c651c 180 g_current_trajectory_time+=samplingPeriod;
labmrd 2:dfeadd6c651c 181
labmrd 2:dfeadd6c651c 182
labmrd 2:dfeadd6c651c 183 moveServoTo(g_theta); // in degrees, son
labmrd 2:dfeadd6c651c 184 #endif
labmrd 2:dfeadd6c651c 185
labmrd 2:dfeadd6c651c 186 //done thinking
labmrd 2:dfeadd6c651c 187 led1 = 0;
labmrd 2:dfeadd6c651c 188 led2 = 1;
labmrd 2:dfeadd6c651c 189 triggerOut = 0;
labmrd 2:dfeadd6c651c 190
labmrd 2:dfeadd6c651c 191 ISRDurationTimer.stop();
labmrd 2:dfeadd6c651c 192 current_latency=ISRDurationTimer.read_us();
labmrd 2:dfeadd6c651c 193 if(current_latency>worst_latency){
labmrd 2:dfeadd6c651c 194 worst_latency=current_latency;
labmrd 2:dfeadd6c651c 195 }
labmrd 2:dfeadd6c651c 196 }
mbed_official 0:8a555873b7d3 197 }
labmrd 2:dfeadd6c651c 198
labmrd 2:dfeadd6c651c 199
mbed_official 0:8a555873b7d3 200 int main() {
labmrd 2:dfeadd6c651c 201 // Crazy fast baud rate!
labmrd 2:dfeadd6c651c 202 pc.baud(921600);
labmrd 2:dfeadd6c651c 203
labmrd 2:dfeadd6c651c 204 #ifdef USE_BLUETOOTH
labmrd 2:dfeadd6c651c 205 bt.baud(9600);
labmrd 2:dfeadd6c651c 206 #endif
labmrd 2:dfeadd6c651c 207
labmrd 2:dfeadd6c651c 208 // Attach ISR routines
labmrd 2:dfeadd6c651c 209 potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod
labmrd 2:dfeadd6c651c 210
labmrd 2:dfeadd6c651c 211 // Some debug info:
labmrd 2:dfeadd6c651c 212 //DisplayRAMBanks();
labmrd 2:dfeadd6c651c 213 //printf ("System clock = %d\r\n", SystemCoreClock);
labmrd 2:dfeadd6c651c 214
labmrd 2:dfeadd6c651c 215 pc.printf("\n\n\n");
labmrd 2:dfeadd6c651c 216 pc.printf("----------------------------------\n");
labmrd 2:dfeadd6c651c 217 pc.printf("| |\n");
labmrd 2:dfeadd6c651c 218 pc.printf("| Welcome to our mbed! |\n");
labmrd 2:dfeadd6c651c 219 pc.printf("| |\n");
labmrd 2:dfeadd6c651c 220 pc.printf("| John and Trevor, Proprietors |\n");
labmrd 2:dfeadd6c651c 221 pc.printf("| |\n");
labmrd 2:dfeadd6c651c 222 pc.printf("----------------------------------\n");
labmrd 2:dfeadd6c651c 223 pc.printf(" ||\n");
labmrd 2:dfeadd6c651c 224 pc.printf(" ||\n");
labmrd 2:dfeadd6c651c 225 pc.printf(" || _\n");
labmrd 2:dfeadd6c651c 226 pc.printf(" || _( )_\n");
labmrd 2:dfeadd6c651c 227 pc.printf(" || (_(#)_)\n");
labmrd 2:dfeadd6c651c 228 pc.printf(" || (_)\\\n");
labmrd 2:dfeadd6c651c 229 pc.printf(" || | __\n");
labmrd 2:dfeadd6c651c 230 pc.printf(" \\ / | || |/_/\n");
labmrd 2:dfeadd6c651c 231 pc.printf(" / | | / / / | || | \\ \\ | / \n");
labmrd 2:dfeadd6c651c 232 pc.printf("/ / \\ \\ / / / || / | / / \\ \\ \n");
labmrd 2:dfeadd6c651c 233 pc.printf("#################################\n");
labmrd 2:dfeadd6c651c 234 pc.printf("#################################\n");
labmrd 2:dfeadd6c651c 235 pc.printf("\n\n");
labmrd 2:dfeadd6c651c 236
labmrd 2:dfeadd6c651c 237 #ifdef USE_DYNAMIXELS
labmrd 2:dfeadd6c651c 238 //mx12_left_jaw.SetBaud(3000000);
labmrd 2:dfeadd6c651c 239 //mx12_left_jaw.SetBaud(1000000);
labmrd 2:dfeadd6c651c 240 //printf("Current Position=%1.3f\n",mx12_left_jaw.GetPosition());
labmrd 2:dfeadd6c651c 241 mx12_left_jaw.Set_Return_Delay_Time(0.002);
labmrd 2:dfeadd6c651c 242 mx12_right_jaw.Set_Return_Delay_Time(0.002);
labmrd 2:dfeadd6c651c 243 mx12_left_jaw.Set_Torque_Limit(30);
labmrd 2:dfeadd6c651c 244 mx12_right_jaw.Set_Torque_Limit(30);
labmrd 2:dfeadd6c651c 245 printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time());
labmrd 2:dfeadd6c651c 246 //mx12_right_jaw.SetReturnDelay(0);
labmrd 2:dfeadd6c651c 247 #else
labmrd 2:dfeadd6c651c 248 // Configure Servo for HiTec 422
labmrd 2:dfeadd6c651c 249 myServoLeft.period_ms(20);
labmrd 2:dfeadd6c651c 250 myServoRight.period_ms(20);
labmrd 2:dfeadd6c651c 251 #endif
labmrd 2:dfeadd6c651c 252
labmrd 2:dfeadd6c651c 253 printf("Setup Complete.\n");
labmrd 2:dfeadd6c651c 254 AuxSerialTimer.start();
labmrd 2:dfeadd6c651c 255
labmrd 2:dfeadd6c651c 256 while(1)
labmrd 2:dfeadd6c651c 257 {
labmrd 2:dfeadd6c651c 258 // spin in a main loop. serialISR will interrupt it to call serialPot
labmrd 2:dfeadd6c651c 259
labmrd 2:dfeadd6c651c 260 ///This checks for any new serial bytes, and returns true if
labmrd 2:dfeadd6c651c 261 ///we have an entire packet ready. The new packet will live
labmrd 2:dfeadd6c651c 262 ///in newData.
labmrd 2:dfeadd6c651c 263 if(
labmrd 2:dfeadd6c651c 264 #ifdef USE_BLUETOOTH
labmrd 2:dfeadd6c651c 265 receivePacket(bt)
labmrd 2:dfeadd6c651c 266 #else
labmrd 2:dfeadd6c651c 267 receivePacket(pc)
labmrd 2:dfeadd6c651c 268 #endif
labmrd 2:dfeadd6c651c 269 )
labmrd 2:dfeadd6c651c 270 {
labmrd 2:dfeadd6c651c 271 // < Tissue Type (string), Frequency Value (Hz) (int), Force Max (N) (int), # Cycles (in) >
labmrd 2:dfeadd6c651c 272 //<date/tissue/time,2,3,4>
labmrd 2:dfeadd6c651c 273 //g_tissue_type_name=tissue_type_name;
labmrd 2:dfeadd6c651c 274 std::string file_name_in=inString.substr(0, inString.find(","));
labmrd 2:dfeadd6c651c 275 g_frequency=newData[1]/10.0; // Since all we send are ints
labmrd 2:dfeadd6c651c 276 g_max_force=newData[2];
labmrd 2:dfeadd6c651c 277 g_num_cycles=newData[3];
labmrd 2:dfeadd6c651c 278 g_current_trajectory_time=0;
labmrd 2:dfeadd6c651c 279 g_current_mode=MODE_AUTOMATIC;
labmrd 2:dfeadd6c651c 280 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 281 int first_slash=file_name_in.find("/");
labmrd 2:dfeadd6c651c 282 std::string new_dir="/sd/"+file_name_in.substr(0, first_slash);
labmrd 2:dfeadd6c651c 283 std::string new_subdir="/sd/"+file_name_in.substr(0, file_name_in.find("/",first_slash+1));
labmrd 2:dfeadd6c651c 284 mkdir(new_dir.c_str(), 0777);
labmrd 2:dfeadd6c651c 285 mkdir(new_subdir.c_str(), 0777);
labmrd 2:dfeadd6c651c 286 std::string file_name="/sd/"+file_name_in+".csv";
labmrd 2:dfeadd6c651c 287 //pc.printf("subdir=\"%s\"\n",file_name.c_str());
labmrd 2:dfeadd6c651c 288 fp = fopen(file_name.c_str(), "w");
labmrd 2:dfeadd6c651c 289 //FILE *fp = fopen("/sd/data/sdtest.txt", "w");
labmrd 2:dfeadd6c651c 290 if(fp == NULL) {
labmrd 2:dfeadd6c651c 291 error("Could not open file for write\n");
labmrd 2:dfeadd6c651c 292 }
labmrd 2:dfeadd6c651c 293 fprintf(fp, "%%Starting New Trajectory\n");
labmrd 2:dfeadd6c651c 294 fprintf(fp, "%%File Name=\"%s\"\n",file_name.c_str());
labmrd 2:dfeadd6c651c 295 fprintf(fp, "%%Current Mode=AUTOMATIC\n");
labmrd 2:dfeadd6c651c 296 fprintf(fp, "%%Frequency=%f Hz\n",g_frequency);
labmrd 2:dfeadd6c651c 297 fprintf(fp, "%%Max Force=%f ??\n",g_max_force);
labmrd 2:dfeadd6c651c 298 fprintf(fp, "%%Num Cycles=%d\n",g_num_cycles);
labmrd 2:dfeadd6c651c 299 fprintf(fp, "%%Re. Direction: ,Closing=%d,Opening=%d,Undef=%d\n", DIRECTION_CLOSING , DIRECTION_OPENING , DIRECTION_SLACK_WATER );
labmrd 2:dfeadd6c651c 300 fprintf(fp, "%%PositionLeft,ForceLeft,PositionRight,ForceRight,Time(ms),Direction,CycleNum\n");
labmrd 2:dfeadd6c651c 301 #endif
labmrd 2:dfeadd6c651c 302 // We are go-times!
labmrd 2:dfeadd6c651c 303 collect_data=true;
labmrd 2:dfeadd6c651c 304 }
labmrd 2:dfeadd6c651c 305
labmrd 2:dfeadd6c651c 306 if( collect_data && g_current_trajectory_time * g_frequency > g_num_cycles)
labmrd 2:dfeadd6c651c 307 {
labmrd 2:dfeadd6c651c 308 // STOOOOOOOOOP
labmrd 2:dfeadd6c651c 309 collect_data=false;
labmrd 2:dfeadd6c651c 310 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 311 // Close the file
labmrd 2:dfeadd6c651c 312 fclose(fp);
labmrd 2:dfeadd6c651c 313 fp = NULL;
labmrd 2:dfeadd6c651c 314 #endif
labmrd 2:dfeadd6c651c 315 }
labmrd 2:dfeadd6c651c 316
labmrd 2:dfeadd6c651c 317 // This section of code should run whenever there is free time to print to the screen
labmrd 2:dfeadd6c651c 318 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 319 if(fp != NULL) {
labmrd 2:dfeadd6c651c 320 // Only write to SD if there is a valid file handle
labmrd 2:dfeadd6c651c 321 led3 = 1;
labmrd 2:dfeadd6c651c 322 Buffer.dumpBufferToSD(fp);
labmrd 2:dfeadd6c651c 323 led3 = 0;
labmrd 2:dfeadd6c651c 324 }
labmrd 2:dfeadd6c651c 325 #else
labmrd 2:dfeadd6c651c 326 Buffer.dumpBufferToSerial();
labmrd 2:dfeadd6c651c 327 #endif
labmrd 2:dfeadd6c651c 328
labmrd 2:dfeadd6c651c 329 if(AuxSerialTimer.read_ms()>100 && collect_data){
labmrd 2:dfeadd6c651c 330 //Send some extra data for GUI purposes
labmrd 2:dfeadd6c651c 331 printf("<%d,%d,%d,%d,%d,%d,%d>\n",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos,
labmrd 2:dfeadd6c651c 332 tempSpindleData.myServoData[LEFT_SERVO_INDEX].force,
labmrd 2:dfeadd6c651c 333 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos,
labmrd 2:dfeadd6c651c 334 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force,
labmrd 2:dfeadd6c651c 335 tempSpindleData.time,
labmrd 2:dfeadd6c651c 336 tempSpindleData.direction,
labmrd 2:dfeadd6c651c 337 tempSpindleData.cycle);
labmrd 2:dfeadd6c651c 338 printf("The worst time taken was %d microseconds\n", worst_latency);
labmrd 2:dfeadd6c651c 339 worst_latency=0;
labmrd 2:dfeadd6c651c 340 AuxSerialTimer.reset();
labmrd 2:dfeadd6c651c 341 }
labmrd 2:dfeadd6c651c 342
mbed_official 0:8a555873b7d3 343 }
mbed_official 0:8a555873b7d3 344 }