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@4:e44ac08027bd, 2015-04-06 (annotated)

Committer:
labmrd
Date:
Mon Apr 06 21:23:36 2015 +0000
Revision:
4:e44ac08027bd
Parent:
2:dfeadd6c651c
Child:
5:72e92c721cd5
This is the state of the art before Rod busted in, Kool Aid Man Style, and blew the whole shebang wigitty-wide open.  From now on, we worship at the alter of the Two State Solution as proposed by Dr. Rod Dockter Master of Science.

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 4:e44ac08027bd 27 #include "AD7730.h"
labmrd 2:dfeadd6c651c 28 #endif
labmrd 2:dfeadd6c651c 29
labmrd 2:dfeadd6c651c 30 // Specific to SD Card
labmrd 2:dfeadd6c651c 31 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 32 #include "SDFileSystem.h"
labmrd 2:dfeadd6c651c 33 #endif
labmrd 2:dfeadd6c651c 34
labmrd 2:dfeadd6c651c 35 // Everyone should know pi...
labmrd 2:dfeadd6c651c 36 #ifndef M_PI
labmrd 2:dfeadd6c651c 37 #define M_PI 3.14159265358979323846 /* pi */
labmrd 2:dfeadd6c651c 38 #endif
labmrd 2:dfeadd6c651c 39 #ifndef M_PI_2
labmrd 2:dfeadd6c651c 40 #define M_PI_2 1.57079632679489661923 /* pi/2 */
labmrd 2:dfeadd6c651c 41 #endif
labmrd 2:dfeadd6c651c 42
labmrd 4:e44ac08027bd 43 // Create enum for the Jaw state (Closing, hold, opening)
labmrd 4:e44ac08027bd 44 enum jaw_state{
labmrd 4:e44ac08027bd 45 STATE_CLOSING=0,
labmrd 4:e44ac08027bd 46 STATE_CLOSE_HOLD=1,
labmrd 4:e44ac08027bd 47 STATE_OPENING=2,
labmrd 4:e44ac08027bd 48 STATE_OPEN_HOLD=3
labmrd 4:e44ac08027bd 49 };
labmrd 2:dfeadd6c651c 50
labmrd 2:dfeadd6c651c 51
labmrd 2:dfeadd6c651c 52 // Define pins and interrupts
labmrd 2:dfeadd6c651c 53 Ticker potISR; //Define a recurring timer interrupt
labmrd 2:dfeadd6c651c 54 DigitalOut led1(LED1); //Led 1 for debugging purposes
labmrd 2:dfeadd6c651c 55 DigitalOut led2(LED2); //Led 2 for debugging purposes
labmrd 2:dfeadd6c651c 56 DigitalOut led3(LED3); //Led 3 for debugging purposes
labmrd 4:e44ac08027bd 57 //DigitalOut led4(LED4); //Led 4 for debugging purposes
labmrd 2:dfeadd6c651c 58 DigitalOut triggerOut(p11);
labmrd 2:dfeadd6c651c 59 Serial pc(USBTX, USBRX); //Set up serial connection to pc
labmrd 2:dfeadd6c651c 60 #ifdef USE_BLUETOOTH
labmrd 2:dfeadd6c651c 61 Serial bt(p13,p14); //Set up serial connection to bluetooth adapter
labmrd 2:dfeadd6c651c 62 #endif
labmrd 2:dfeadd6c651c 63
labmrd 2:dfeadd6c651c 64 AnalogIn AinLeftForce(p16); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 65 AnalogIn AinRightForce(p15); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 66
labmrd 2:dfeadd6c651c 67 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 68 // Attach SD card
labmrd 2:dfeadd6c651c 69 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
labmrd 2:dfeadd6c651c 70 FILE *fp = NULL;
labmrd 2:dfeadd6c651c 71 #define SAMPLES_PER_FILE 10000
labmrd 2:dfeadd6c651c 72 #endif
labmrd 2:dfeadd6c651c 73
labmrd 2:dfeadd6c651c 74 // Dummy variable for debugging
labmrd 2:dfeadd6c651c 75 unsigned int global_count=0;
labmrd 2:dfeadd6c651c 76 float max_percent_full=0;
labmrd 2:dfeadd6c651c 77
labmrd 2:dfeadd6c651c 78 // Define variables for the program
labmrd 2:dfeadd6c651c 79 float servoAngle; //This is the desired servo angle based on the scaled potentiometer value
labmrd 2:dfeadd6c651c 80 float potData; //This is the value of the potentiometer from Ain.read()
labmrd 2:dfeadd6c651c 81 bool collect_data = false; //This is
labmrd 2:dfeadd6c651c 82
labmrd 2:dfeadd6c651c 83 bool keyStrokeFlag = false; //This is a flag to see if a keystroke has been pressed
labmrd 2:dfeadd6c651c 84 char keyStrokeVal; //This is a character storing the value of the keystroke
labmrd 2:dfeadd6c651c 85
labmrd 2:dfeadd6c651c 86 char g_tissue_type_name[32];
labmrd 2:dfeadd6c651c 87 float g_frequency;
labmrd 2:dfeadd6c651c 88 int g_max_force;
labmrd 2:dfeadd6c651c 89 int g_num_cycles;
labmrd 2:dfeadd6c651c 90 float g_current_trajectory_time;
labmrd 2:dfeadd6c651c 91 float g_theta;
labmrd 2:dfeadd6c651c 92 float g_theta_last=0;
labmrd 2:dfeadd6c651c 93 unsigned char g_current_mode=MODE_NULL;
labmrd 4:e44ac08027bd 94 jaw_state g_current_direction=STATE_OPEN_HOLD;
labmrd 2:dfeadd6c651c 95 unsigned char g_current_cycle=0;
labmrd 2:dfeadd6c651c 96
labmrd 2:dfeadd6c651c 97 // Warning, this buffer is large!
labmrd 2:dfeadd6c651c 98 ServoRingBuffer Buffer;
labmrd 2:dfeadd6c651c 99 spindleData tempSpindleData; //For sending to the buffer
labmrd 2:dfeadd6c651c 100
labmrd 2:dfeadd6c651c 101 Timer ISRDurationTimer;
labmrd 2:dfeadd6c651c 102 Timer AuxSerialTimer;
labmrd 2:dfeadd6c651c 103 int worst_latency=0;
labmrd 2:dfeadd6c651c 104 int current_latency;
labmrd 2:dfeadd6c651c 105
labmrd 2:dfeadd6c651c 106 #ifdef USE_DYNAMIXELS
labmrd 2:dfeadd6c651c 107 //Dynamixels can only handle 500Hz for now. Working on it...
labmrd 4:e44ac08027bd 108 float samplingPeriod = 0.005; //This is the sampling period for the timer interrupt
labmrd 2:dfeadd6c651c 109 #define LEFT_JAW_DYNAMIXEL_ID 3
labmrd 2:dfeadd6c651c 110 #define RIGHT_JAW_DYNAMIXEL_ID 4
labmrd 4:e44ac08027bd 111 #define CLOSED_SERVO_ANGLE_LEFT 1121 //This is the closed in encoder counts
labmrd 4:e44ac08027bd 112 #define OPEN_SERVO_ANGLE_LEFT 2783 //This is the open in encoder counts
labmrd 4:e44ac08027bd 113 #define CLOSED_SERVO_ANGLE_RIGHT 3259 //This is the closed in encoder counts
labmrd 4:e44ac08027bd 114 #define OPEN_SERVO_ANGLE_RIGHT 1486 //This is the open in encoder counts
labmrd 2:dfeadd6c651c 115 // Dynamixel Object
labmrd 4:e44ac08027bd 116 MX12 mx12_left_jaw (p28, p27, p30, p29, LEFT_JAW_DYNAMIXEL_ID, 1000000);
labmrd 4:e44ac08027bd 117 MX12 mx12_right_jaw (p28, p27, p30, p29, RIGHT_JAW_DYNAMIXEL_ID, 1000000);
labmrd 2:dfeadd6c651c 118
labmrd 4:e44ac08027bd 119 AD7730 adc(p9, p26, p11, p12, p25);
labmrd 4:e44ac08027bd 120
labmrd 4:e44ac08027bd 121 /// Set these to inputs so that they don't interfere with the serial communication
labmrd 2:dfeadd6c651c 122 DigitalIn nullOut1(p21);
labmrd 2:dfeadd6c651c 123 DigitalIn nullOut2(p22);
labmrd 2:dfeadd6c651c 124 DigitalIn nullOut3(p23);
labmrd 2:dfeadd6c651c 125 DigitalIn nullOut4(p24);
labmrd 2:dfeadd6c651c 126 #else
labmrd 2:dfeadd6c651c 127 float samplingPeriod = 0.001; //This is the sampling period for the timer interrupt
labmrd 2:dfeadd6c651c 128 #define SERVO_DEGREE_0 900 //This is the pulse width value for HiTEC-422 in microseconds to turn 0 degrees
labmrd 2:dfeadd6c651c 129 #define SERVO_DEGREE_180 2100 //This is the pulse width value for HiTEC-422 in microseconds to turn 180 degrees
labmrd 2:dfeadd6c651c 130 #define MIN_SERVO_ANGLE 0.0 //This is the minimum servo angle in degrees
labmrd 2:dfeadd6c651c 131 #define MAX_SERVO_ANGLE 180.0 //This is the maximum servo angle in degrees
labmrd 2:dfeadd6c651c 132 #define MIN_SERVO_ANGLE_Da_VINCI 20.0 //This is the minimum servo angle in degrees
labmrd 2:dfeadd6c651c 133 #define MAX_SERVO_ANGLE_Da_VINCI 100.0 //This is the maximum servo angle in degrees
labmrd 2:dfeadd6c651c 134 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 135 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 136
labmrd 2:dfeadd6c651c 137 PwmOut myServoLeft(p21); //Set up servo on pin 21
labmrd 2:dfeadd6c651c 138 PwmOut myServoRight(p22); //Set up servo on pin 22
labmrd 2:dfeadd6c651c 139 AnalogIn AinLeftPosition(p20); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 140 AnalogIn AinRightPosition(p19); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 141
labmrd 2:dfeadd6c651c 142
labmrd 2:dfeadd6c651c 143 // Function moveServoTo: Convert a degree value to pulsewidth for Servo
labmrd 2:dfeadd6c651c 144 void moveServoTo(float angle) {
labmrd 2:dfeadd6c651c 145 // Make sure none of the user input falls outside of min and max angle limits
labmrd 2:dfeadd6c651c 146 if( angle < MIN_SERVO_ANGLE){angle = MIN_SERVO_ANGLE;}
labmrd 2:dfeadd6c651c 147 else if(angle > MAX_SERVO_ANGLE){angle = MAX_SERVO_ANGLE;}
labmrd 2:dfeadd6c651c 148 myServoLeft.pulsewidth(servoOffset + servoConversion*(180-angle));
labmrd 2:dfeadd6c651c 149 myServoRight.pulsewidth(servoOffset + servoConversion*(angle));
labmrd 2:dfeadd6c651c 150 }
labmrd 2:dfeadd6c651c 151
labmrd 2:dfeadd6c651c 152 #endif
labmrd 2:dfeadd6c651c 153
labmrd 4:e44ac08027bd 154 // Function trapezoidalTrajectory: Function that takes in a time (float in seconds) and outputs a float (0 to 1) that corresponds to a trapezoidal trajectory
labmrd 4:e44ac08027bd 155 float trapezoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) {
labmrd 4:e44ac08027bd 156 // Define variables specific to this function
labmrd 4:e44ac08027bd 157 float y_trapezoid = 0.0;
labmrd 4:e44ac08027bd 158 float timeMod;
labmrd 4:e44ac08027bd 159 float modifiedFrequency = g_frequency/2.0;
labmrd 4:e44ac08027bd 160 float period = 1/modifiedFrequency;
labmrd 4:e44ac08027bd 161 cycle_num=t*modifiedFrequency;
labmrd 4:e44ac08027bd 162
labmrd 4:e44ac08027bd 163 // Take the time and mod it with the period to be able to break up each cycle into 4 piecewise sections
labmrd 4:e44ac08027bd 164 timeMod = fmodf(t,period);
labmrd 4:e44ac08027bd 165
labmrd 4:e44ac08027bd 166 //
labmrd 4:e44ac08027bd 167 if (timeMod < period/4.0)
labmrd 4:e44ac08027bd 168 {
labmrd 4:e44ac08027bd 169 y_trapezoid = (-4.0/period)*(timeMod)+1.0;
labmrd 4:e44ac08027bd 170 state = STATE_CLOSING;
labmrd 4:e44ac08027bd 171 }
labmrd 4:e44ac08027bd 172 else if (timeMod >= period/4.0 && timeMod < period/2.0)
labmrd 4:e44ac08027bd 173 {
labmrd 4:e44ac08027bd 174 y_trapezoid = 0.0;
labmrd 4:e44ac08027bd 175 state = STATE_CLOSE_HOLD;
labmrd 4:e44ac08027bd 176 }
labmrd 4:e44ac08027bd 177 else if (timeMod >= period/2.0 && timeMod < 3*period/4.0)
labmrd 4:e44ac08027bd 178 {
labmrd 4:e44ac08027bd 179 y_trapezoid = (4.0/period)*(timeMod)-2;
labmrd 4:e44ac08027bd 180 state = STATE_OPENING;
labmrd 4:e44ac08027bd 181 }
labmrd 4:e44ac08027bd 182 else if (timeMod >= 3*period/4.0)
labmrd 4:e44ac08027bd 183 {
labmrd 4:e44ac08027bd 184 y_trapezoid = 1.0;
labmrd 4:e44ac08027bd 185 state = STATE_OPEN_HOLD;
labmrd 4:e44ac08027bd 186 }
labmrd 4:e44ac08027bd 187
labmrd 4:e44ac08027bd 188 return y_trapezoid;
labmrd 4:e44ac08027bd 189 }
labmrd 4:e44ac08027bd 190
labmrd 4:e44ac08027bd 191 void sinusoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) {
labmrd 4:e44ac08027bd 192 //Fill me with SCIENCE!!!
labmrd 4:e44ac08027bd 193 }
labmrd 4:e44ac08027bd 194
labmrd 4:e44ac08027bd 195
labmrd 2:dfeadd6c651c 196 // Function timerISRFunction: Timer ISR function to collect data and write to ring buffer
labmrd 2:dfeadd6c651c 197 void timerISRFunction() {
labmrd 2:dfeadd6c651c 198 if(collect_data){
labmrd 2:dfeadd6c651c 199 //led 1 is used as a 'thinking' light, brighter=worse
labmrd 2:dfeadd6c651c 200 led1 = 1;
labmrd 2:dfeadd6c651c 201 led2 = 0;
labmrd 2:dfeadd6c651c 202 triggerOut = 1;
labmrd 2:dfeadd6c651c 203
labmrd 2:dfeadd6c651c 204 ISRDurationTimer.reset();
labmrd 2:dfeadd6c651c 205 ISRDurationTimer.start();
labmrd 2:dfeadd6c651c 206
labmrd 2:dfeadd6c651c 207 // Warning, this calculation is in the ISR and as such is probably slower than we would prefer.
labmrd 2:dfeadd6c651c 208 // @todo The math could certainly be optimized with some precalculated constants. Lookup tables are faster than sin()
labmrd 4:e44ac08027bd 209 float percent=trapezoidalTrajectory(g_current_trajectory_time,g_current_direction,g_current_cycle);
labmrd 4:e44ac08027bd 210 g_current_trajectory_time+=samplingPeriod;
labmrd 4:e44ac08027bd 211
labmrd 4:e44ac08027bd 212
labmrd 4:e44ac08027bd 213 //float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
labmrd 4:e44ac08027bd 214 //g_current_direction=(cos(angle)<0);
labmrd 4:e44ac08027bd 215 //g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
labmrd 4:e44ac08027bd 216
labmrd 2:dfeadd6c651c 217
labmrd 2:dfeadd6c651c 218 #ifdef USE_DYNAMIXELS
labmrd 4:e44ac08027bd 219 //float percent=(sin(angle)+1)/2.0;
labmrd 4:e44ac08027bd 220 if(adc.isReady()){
labmrd 4:e44ac08027bd 221 adc.interruptRead();
labmrd 4:e44ac08027bd 222 }
labmrd 4:e44ac08027bd 223
labmrd 4:e44ac08027bd 224 short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
labmrd 4:e44ac08027bd 225 short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
labmrd 4:e44ac08027bd 226 mx12_right_jaw.coordinated_move(LEFT_JAW_DYNAMIXEL_ID,left_servo, 0, RIGHT_JAW_DYNAMIXEL_ID,right_servo, 0);
labmrd 4:e44ac08027bd 227
labmrd 4:e44ac08027bd 228 // tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = adc.read();
labmrd 4:e44ac08027bd 229 // tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = mx12_left_jaw.GetRawPosition();
labmrd 4:e44ac08027bd 230 // tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
labmrd 4:e44ac08027bd 231 // tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = mx12_right_jaw.GetRawPosition();
labmrd 4:e44ac08027bd 232 // tempSpindleData.direction=g_current_direction;
labmrd 4:e44ac08027bd 233 // tempSpindleData.cycle=g_current_cycle;
labmrd 4:e44ac08027bd 234 // Buffer.write(tempSpindleData);
labmrd 2:dfeadd6c651c 235 #else
labmrd 4:e44ac08027bd 236 g_theta=(1.0-percent)*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI;
labmrd 2:dfeadd6c651c 237 tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = AinLeftForce.read_u16();
labmrd 2:dfeadd6c651c 238 tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = AinLeftPosition.read_u16();
labmrd 2:dfeadd6c651c 239 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
labmrd 2:dfeadd6c651c 240 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = AinRightPosition.read_u16();
labmrd 2:dfeadd6c651c 241 tempSpindleData.direction=g_current_direction;
labmrd 2:dfeadd6c651c 242 tempSpindleData.cycle=g_current_cycle;
labmrd 2:dfeadd6c651c 243 Buffer.write(tempSpindleData);
labmrd 2:dfeadd6c651c 244
labmrd 2:dfeadd6c651c 245
labmrd 2:dfeadd6c651c 246
labmrd 2:dfeadd6c651c 247 moveServoTo(g_theta); // in degrees, son
labmrd 2:dfeadd6c651c 248 #endif
labmrd 2:dfeadd6c651c 249
labmrd 2:dfeadd6c651c 250 //done thinking
labmrd 2:dfeadd6c651c 251 led1 = 0;
labmrd 2:dfeadd6c651c 252 led2 = 1;
labmrd 2:dfeadd6c651c 253 triggerOut = 0;
labmrd 2:dfeadd6c651c 254
labmrd 2:dfeadd6c651c 255 ISRDurationTimer.stop();
labmrd 2:dfeadd6c651c 256 current_latency=ISRDurationTimer.read_us();
labmrd 2:dfeadd6c651c 257 if(current_latency>worst_latency){
labmrd 2:dfeadd6c651c 258 worst_latency=current_latency;
labmrd 2:dfeadd6c651c 259 }
labmrd 2:dfeadd6c651c 260 }
mbed_official 0:8a555873b7d3 261 }
labmrd 2:dfeadd6c651c 262
labmrd 2:dfeadd6c651c 263
mbed_official 0:8a555873b7d3 264 int main() {
labmrd 2:dfeadd6c651c 265 // Crazy fast baud rate!
labmrd 2:dfeadd6c651c 266 pc.baud(921600);
labmrd 2:dfeadd6c651c 267
labmrd 2:dfeadd6c651c 268 #ifdef USE_BLUETOOTH
labmrd 2:dfeadd6c651c 269 bt.baud(9600);
labmrd 2:dfeadd6c651c 270 #endif
labmrd 2:dfeadd6c651c 271
labmrd 2:dfeadd6c651c 272 // Attach ISR routines
labmrd 2:dfeadd6c651c 273 potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod
labmrd 2:dfeadd6c651c 274
labmrd 2:dfeadd6c651c 275 // Some debug info:
labmrd 2:dfeadd6c651c 276 //DisplayRAMBanks();
labmrd 2:dfeadd6c651c 277 //printf ("System clock = %d\r\n", SystemCoreClock);
labmrd 2:dfeadd6c651c 278
labmrd 2:dfeadd6c651c 279 pc.printf("\n\n\n");
labmrd 2:dfeadd6c651c 280 pc.printf("----------------------------------\n");
labmrd 2:dfeadd6c651c 281 pc.printf("| |\n");
labmrd 2:dfeadd6c651c 282 pc.printf("| Welcome to our mbed! |\n");
labmrd 2:dfeadd6c651c 283 pc.printf("| |\n");
labmrd 2:dfeadd6c651c 284 pc.printf("| John and Trevor, Proprietors |\n");
labmrd 2:dfeadd6c651c 285 pc.printf("| |\n");
labmrd 2:dfeadd6c651c 286 pc.printf("----------------------------------\n");
labmrd 2:dfeadd6c651c 287 pc.printf(" ||\n");
labmrd 2:dfeadd6c651c 288 pc.printf(" ||\n");
labmrd 2:dfeadd6c651c 289 pc.printf(" || _\n");
labmrd 2:dfeadd6c651c 290 pc.printf(" || _( )_\n");
labmrd 2:dfeadd6c651c 291 pc.printf(" || (_(#)_)\n");
labmrd 2:dfeadd6c651c 292 pc.printf(" || (_)\\\n");
labmrd 2:dfeadd6c651c 293 pc.printf(" || | __\n");
labmrd 2:dfeadd6c651c 294 pc.printf(" \\ / | || |/_/\n");
labmrd 2:dfeadd6c651c 295 pc.printf(" / | | / / / | || | \\ \\ | / \n");
labmrd 2:dfeadd6c651c 296 pc.printf("/ / \\ \\ / / / || / | / / \\ \\ \n");
labmrd 2:dfeadd6c651c 297 pc.printf("#################################\n");
labmrd 2:dfeadd6c651c 298 pc.printf("#################################\n");
labmrd 2:dfeadd6c651c 299 pc.printf("\n\n");
labmrd 2:dfeadd6c651c 300
labmrd 2:dfeadd6c651c 301 #ifdef USE_DYNAMIXELS
labmrd 4:e44ac08027bd 302 mx12_left_jaw.Init();
labmrd 2:dfeadd6c651c 303 //mx12_left_jaw.SetBaud(3000000);
labmrd 2:dfeadd6c651c 304 //mx12_left_jaw.SetBaud(1000000);
labmrd 2:dfeadd6c651c 305 //printf("Current Position=%1.3f\n",mx12_left_jaw.GetPosition());
labmrd 4:e44ac08027bd 306 mx12_right_jaw.Set_Return_Delay_Time(0.050);
labmrd 2:dfeadd6c651c 307 printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time());
labmrd 4:e44ac08027bd 308 mx12_left_jaw.Set_Return_Delay_Time(0.050);
labmrd 4:e44ac08027bd 309 //mx12_left_jaw.Set_Torque_Limit(99.9);
labmrd 4:e44ac08027bd 310 //mx12_right_jaw.Set_Torque_Limit(99.9);
labmrd 4:e44ac08027bd 311 mx12_left_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
labmrd 4:e44ac08027bd 312 mx12_right_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
labmrd 4:e44ac08027bd 313 mx12_left_jaw.Set_P_Gain(4);
labmrd 4:e44ac08027bd 314 mx12_right_jaw.Set_P_Gain(4);
labmrd 4:e44ac08027bd 315 mx12_left_jaw.Set_I_Gain(8);
labmrd 4:e44ac08027bd 316 mx12_right_jaw.Set_I_Gain(8);
labmrd 4:e44ac08027bd 317 mx12_left_jaw.Set_Alarm_Shutdown(0x04);
labmrd 4:e44ac08027bd 318 mx12_right_jaw.Set_Alarm_Shutdown(0x04);
labmrd 4:e44ac08027bd 319
labmrd 4:e44ac08027bd 320 mx12_left_jaw.Dump_OD_to_Serial(pc);
labmrd 4:e44ac08027bd 321 mx12_right_jaw.Dump_OD_to_Serial(pc);
labmrd 4:e44ac08027bd 322
labmrd 4:e44ac08027bd 323
labmrd 4:e44ac08027bd 324
labmrd 4:e44ac08027bd 325 adc.setFilter(256 , false, 1);
labmrd 4:e44ac08027bd 326 adc.start();
labmrd 2:dfeadd6c651c 327 #else
labmrd 2:dfeadd6c651c 328 // Configure Servo for HiTec 422
labmrd 2:dfeadd6c651c 329 myServoLeft.period_ms(20);
labmrd 2:dfeadd6c651c 330 myServoRight.period_ms(20);
labmrd 2:dfeadd6c651c 331 #endif
labmrd 2:dfeadd6c651c 332
labmrd 2:dfeadd6c651c 333 printf("Setup Complete.\n");
labmrd 2:dfeadd6c651c 334 AuxSerialTimer.start();
labmrd 2:dfeadd6c651c 335
labmrd 2:dfeadd6c651c 336 while(1)
labmrd 2:dfeadd6c651c 337 {
labmrd 2:dfeadd6c651c 338 // spin in a main loop. serialISR will interrupt it to call serialPot
labmrd 2:dfeadd6c651c 339
labmrd 2:dfeadd6c651c 340 ///This checks for any new serial bytes, and returns true if
labmrd 2:dfeadd6c651c 341 ///we have an entire packet ready. The new packet will live
labmrd 2:dfeadd6c651c 342 ///in newData.
labmrd 2:dfeadd6c651c 343 if(
labmrd 2:dfeadd6c651c 344 #ifdef USE_BLUETOOTH
labmrd 2:dfeadd6c651c 345 receivePacket(bt)
labmrd 2:dfeadd6c651c 346 #else
labmrd 2:dfeadd6c651c 347 receivePacket(pc)
labmrd 2:dfeadd6c651c 348 #endif
labmrd 2:dfeadd6c651c 349 )
labmrd 2:dfeadd6c651c 350 {
labmrd 2:dfeadd6c651c 351 // < Tissue Type (string), Frequency Value (Hz) (int), Force Max (N) (int), # Cycles (in) >
labmrd 2:dfeadd6c651c 352 //<date/tissue/time,2,3,4>
labmrd 2:dfeadd6c651c 353 //g_tissue_type_name=tissue_type_name;
labmrd 2:dfeadd6c651c 354 std::string file_name_in=inString.substr(0, inString.find(","));
labmrd 2:dfeadd6c651c 355 g_frequency=newData[1]/10.0; // Since all we send are ints
labmrd 2:dfeadd6c651c 356 g_max_force=newData[2];
labmrd 2:dfeadd6c651c 357 g_num_cycles=newData[3];
labmrd 2:dfeadd6c651c 358 g_current_trajectory_time=0;
labmrd 4:e44ac08027bd 359 g_current_cycle=0;
labmrd 2:dfeadd6c651c 360 g_current_mode=MODE_AUTOMATIC;
labmrd 2:dfeadd6c651c 361 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 362 int first_slash=file_name_in.find("/");
labmrd 2:dfeadd6c651c 363 std::string new_dir="/sd/"+file_name_in.substr(0, first_slash);
labmrd 2:dfeadd6c651c 364 std::string new_subdir="/sd/"+file_name_in.substr(0, file_name_in.find("/",first_slash+1));
labmrd 2:dfeadd6c651c 365 mkdir(new_dir.c_str(), 0777);
labmrd 2:dfeadd6c651c 366 mkdir(new_subdir.c_str(), 0777);
labmrd 2:dfeadd6c651c 367 std::string file_name="/sd/"+file_name_in+".csv";
labmrd 2:dfeadd6c651c 368 //pc.printf("subdir=\"%s\"\n",file_name.c_str());
labmrd 2:dfeadd6c651c 369 fp = fopen(file_name.c_str(), "w");
labmrd 2:dfeadd6c651c 370 //FILE *fp = fopen("/sd/data/sdtest.txt", "w");
labmrd 2:dfeadd6c651c 371 if(fp == NULL) {
labmrd 2:dfeadd6c651c 372 error("Could not open file for write\n");
labmrd 2:dfeadd6c651c 373 }
labmrd 2:dfeadd6c651c 374 fprintf(fp, "%%Starting New Trajectory\n");
labmrd 2:dfeadd6c651c 375 fprintf(fp, "%%File Name=\"%s\"\n",file_name.c_str());
labmrd 2:dfeadd6c651c 376 fprintf(fp, "%%Current Mode=AUTOMATIC\n");
labmrd 4:e44ac08027bd 377 fprintf(fp, "%%Trajectory Type=TRAPEZOIDAL\n");
labmrd 2:dfeadd6c651c 378 fprintf(fp, "%%Frequency=%f Hz\n",g_frequency);
labmrd 2:dfeadd6c651c 379 fprintf(fp, "%%Max Force=%f ??\n",g_max_force);
labmrd 2:dfeadd6c651c 380 fprintf(fp, "%%Num Cycles=%d\n",g_num_cycles);
labmrd 2:dfeadd6c651c 381 fprintf(fp, "%%Re. Direction: ,Closing=%d,Opening=%d,Undef=%d\n", DIRECTION_CLOSING , DIRECTION_OPENING , DIRECTION_SLACK_WATER );
labmrd 2:dfeadd6c651c 382 fprintf(fp, "%%PositionLeft,ForceLeft,PositionRight,ForceRight,Time(ms),Direction,CycleNum\n");
labmrd 2:dfeadd6c651c 383 #endif
labmrd 2:dfeadd6c651c 384 // We are go-times!
labmrd 2:dfeadd6c651c 385 collect_data=true;
labmrd 2:dfeadd6c651c 386 }
labmrd 2:dfeadd6c651c 387
labmrd 4:e44ac08027bd 388 if( collect_data && g_current_cycle >= g_num_cycles)
labmrd 2:dfeadd6c651c 389 {
labmrd 2:dfeadd6c651c 390 // STOOOOOOOOOP
labmrd 2:dfeadd6c651c 391 collect_data=false;
labmrd 2:dfeadd6c651c 392 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 393 // Close the file
labmrd 2:dfeadd6c651c 394 fclose(fp);
labmrd 2:dfeadd6c651c 395 fp = NULL;
labmrd 2:dfeadd6c651c 396 #endif
labmrd 2:dfeadd6c651c 397 }
labmrd 2:dfeadd6c651c 398
labmrd 2:dfeadd6c651c 399 // This section of code should run whenever there is free time to print to the screen
labmrd 2:dfeadd6c651c 400 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 401 if(fp != NULL) {
labmrd 2:dfeadd6c651c 402 // Only write to SD if there is a valid file handle
labmrd 2:dfeadd6c651c 403 led3 = 1;
labmrd 2:dfeadd6c651c 404 Buffer.dumpBufferToSD(fp);
labmrd 2:dfeadd6c651c 405 led3 = 0;
labmrd 2:dfeadd6c651c 406 }
labmrd 2:dfeadd6c651c 407 #else
labmrd 2:dfeadd6c651c 408 Buffer.dumpBufferToSerial();
labmrd 2:dfeadd6c651c 409 #endif
labmrd 2:dfeadd6c651c 410
labmrd 2:dfeadd6c651c 411 if(AuxSerialTimer.read_ms()>100 && collect_data){
labmrd 2:dfeadd6c651c 412 //Send some extra data for GUI purposes
labmrd 4:e44ac08027bd 413 printf("<%d,%d,%d,%d,%d,%d,%d> ",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos,
labmrd 2:dfeadd6c651c 414 tempSpindleData.myServoData[LEFT_SERVO_INDEX].force,
labmrd 2:dfeadd6c651c 415 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos,
labmrd 2:dfeadd6c651c 416 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force,
labmrd 2:dfeadd6c651c 417 tempSpindleData.time,
labmrd 2:dfeadd6c651c 418 tempSpindleData.direction,
labmrd 2:dfeadd6c651c 419 tempSpindleData.cycle);
labmrd 4:e44ac08027bd 420 printf(" %dus\n", worst_latency);
labmrd 2:dfeadd6c651c 421 worst_latency=0;
labmrd 2:dfeadd6c651c 422 AuxSerialTimer.reset();
labmrd 2:dfeadd6c651c 423 }
labmrd 2:dfeadd6c651c 424
mbed_official 0:8a555873b7d3 425 }
mbed_official 0:8a555873b7d3 426 }