MRD Lab / SpindleBot_1_5b

A code for the spindling of bots.

Dependencies:   MX12 ServoRingBuffer mbed-src

Fork of SpindleBot by MRD Lab

main.cpp@14:7c5beaa9fb01, 2015-08-13 (annotated)

Committer:
labmrd
Date:
Thu Aug 13 17:55:40 2015 +0000
Revision:
14:7c5beaa9fb01
Parent:
13:6a0a7a04fd91 
This revision marks Mark's mark of resignation from the labmrd mbed account.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
labmrd 14:7c5beaa9fb01 1 //#define USE_DYNAMIXELS
labmrd 14:7c5beaa9fb01 2 #define USE_BLUETOOTH
labmrd 14:7c5beaa9fb01 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 14:7c5beaa9fb01 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 14:7c5beaa9fb01 51
labmrd 2:dfeadd6c651c 52 // Define pins and interrupts
labmrd 14:7c5beaa9fb01 53 Ticker potISR; //Define a recurring timer interrupt
labmrd 14:7c5beaa9fb01 54 DigitalOut led1(LED1); //Led 1 for debugging purposes
labmrd 14:7c5beaa9fb01 55 DigitalOut led2(LED2); //Led 2 for debugging purposes
labmrd 14:7c5beaa9fb01 56 DigitalOut led3(LED3); //Led 3 for debugging purposes
labmrd 14:7c5beaa9fb01 57 //DigitalOut led4(LED4); //Led 4 for debugging purposes
labmrd 2:dfeadd6c651c 58 DigitalOut triggerOut(p11);
labmrd 14:7c5beaa9fb01 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 14:7c5beaa9fb01 64 AnalogIn AinLeftForce(p16); //Set up potentiometer on pin 20
labmrd 14:7c5beaa9fb01 65 AnalogIn AinRightForce(p15); //Set up potentiometer on pin 20
labmrd 14:7c5beaa9fb01 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 14:7c5beaa9fb01 79 float servoAngle; //This is the desired servo angle based on the scaled potentiometer value
labmrd 14:7c5beaa9fb01 80 float potData; //This is the value of the potentiometer from Ain.read()
labmrd 14:7c5beaa9fb01 81 bool collect_data = false; //This is
labmrd 14:7c5beaa9fb01 82
labmrd 14:7c5beaa9fb01 83 bool keyStrokeFlag = false; //This is a flag to see if a keystroke has been pressed
labmrd 14:7c5beaa9fb01 84 char keyStrokeVal; //This is a character storing the value of the keystroke
labmrd 14:7c5beaa9fb01 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 14:7c5beaa9fb01 107 //Dynamixels can only handle 500Hz for now. Working on it...
labmrd 14:7c5beaa9fb01 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 14:7c5beaa9fb01 111 #define CLOSED_SERVO_ANGLE_LEFT 1121 //This is the closed in encoder counts
labmrd 14:7c5beaa9fb01 112 #define OPEN_SERVO_ANGLE_LEFT 2783 //This is the open in encoder counts
labmrd 14:7c5beaa9fb01 113 #define CLOSED_SERVO_ANGLE_RIGHT 3259 //This is the closed in encoder counts
labmrd 14:7c5beaa9fb01 114 #define OPEN_SERVO_ANGLE_RIGHT 1486 //This is the open in encoder counts
labmrd 14:7c5beaa9fb01 115 // Dynamixel Object
labmrd 14:7c5beaa9fb01 116 MX12 mx12_left_jaw (p28, p27, p30, p29, LEFT_JAW_DYNAMIXEL_ID, 1000000);
labmrd 14:7c5beaa9fb01 117 MX12 mx12_right_jaw (p28, p27, p30, p29, RIGHT_JAW_DYNAMIXEL_ID, 1000000);
labmrd 2:dfeadd6c651c 118
labmrd 14:7c5beaa9fb01 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 14:7c5beaa9fb01 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 14:7c5beaa9fb01 198 if(collect_data){
labmrd 8:cef07a8bdc42 199 //led 1 is used as a 'thinking' light, brighter=worse
labmrd 8:cef07a8bdc42 200 led1 = 1;
labmrd 8:cef07a8bdc42 201 led2 = 0;
labmrd 8:cef07a8bdc42 202 triggerOut = 1;
labmrd 2:dfeadd6c651c 203
labmrd 8:cef07a8bdc42 204 ISRDurationTimer.reset();
labmrd 8:cef07a8bdc42 205 ISRDurationTimer.start();
labmrd 14:7c5beaa9fb01 206
labmrd 14:7c5beaa9fb01 207 // Warning, this calculation is in the ISR and as such is probably slower than we would prefer.
labmrd 14:7c5beaa9fb01 208 // @todo The math could certainly be optimized with some precalculated constants. Lookup tables are faster than sin()
labmrd 14:7c5beaa9fb01 209 float percent=trapezoidalTrajectory(g_current_trajectory_time,g_current_direction,g_current_cycle);
labmrd 14:7c5beaa9fb01 210 g_current_trajectory_time+=samplingPeriod;
labmrd 14:7c5beaa9fb01 211
labmrd 14:7c5beaa9fb01 212
labmrd 14:7c5beaa9fb01 213 //float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
labmrd 14:7c5beaa9fb01 214 //g_current_direction=(cos(angle)<0);
labmrd 14:7c5beaa9fb01 215 //g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
labmrd 14:7c5beaa9fb01 216
labmrd 14:7c5beaa9fb01 217
labmrd 14:7c5beaa9fb01 218 #ifdef USE_DYNAMIXELS
labmrd 14:7c5beaa9fb01 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 14:7c5beaa9fb01 224 short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
labmrd 14:7c5beaa9fb01 225 short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
labmrd 14:7c5beaa9fb01 226 mx12_right_jaw.coordinated_move(LEFT_JAW_DYNAMIXEL_ID,left_servo, 0, RIGHT_JAW_DYNAMIXEL_ID,right_servo, 0);
labmrd 5:72e92c721cd5 227
labmrd 14:7c5beaa9fb01 228 // tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = adc.read();
labmrd 14:7c5beaa9fb01 229 // tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = mx12_left_jaw.GetRawPosition();
labmrd 14:7c5beaa9fb01 230 // tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
labmrd 14:7c5beaa9fb01 231 // tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = mx12_right_jaw.GetRawPosition();
labmrd 14:7c5beaa9fb01 232 // tempSpindleData.direction=g_current_direction;
labmrd 14:7c5beaa9fb01 233 // tempSpindleData.cycle=g_current_cycle;
labmrd 14:7c5beaa9fb01 234 // Buffer.write(tempSpindleData);
labmrd 14:7c5beaa9fb01 235 #else
labmrd 14:7c5beaa9fb01 236 g_theta=(1.0-percent)*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI;
labmrd 14:7c5beaa9fb01 237 tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = AinLeftForce.read_u16();
labmrd 14:7c5beaa9fb01 238 tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = AinLeftPosition.read_u16();
labmrd 14:7c5beaa9fb01 239 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
labmrd 14:7c5beaa9fb01 240 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = AinRightPosition.read_u16();
labmrd 14:7c5beaa9fb01 241 tempSpindleData.direction=g_current_direction;
labmrd 14:7c5beaa9fb01 242 tempSpindleData.cycle=g_current_cycle;
labmrd 14:7c5beaa9fb01 243 Buffer.write(tempSpindleData);
labmrd 5:72e92c721cd5 244
labmrd 14:7c5beaa9fb01 245
labmrd 2:dfeadd6c651c 246
labmrd 14:7c5beaa9fb01 247 moveServoTo(g_theta); // in degrees, son
labmrd 14:7c5beaa9fb01 248 #endif
labmrd 14:7c5beaa9fb01 249
labmrd 8:cef07a8bdc42 250 //done thinking
labmrd 8:cef07a8bdc42 251 led1 = 0;
labmrd 8:cef07a8bdc42 252 led2 = 1;
labmrd 8:cef07a8bdc42 253 triggerOut = 0;
labmrd 2:dfeadd6c651c 254
labmrd 8:cef07a8bdc42 255 ISRDurationTimer.stop();
labmrd 8:cef07a8bdc42 256 current_latency=ISRDurationTimer.read_us();
labmrd 8:cef07a8bdc42 257 if(current_latency>worst_latency){
labmrd 8:cef07a8bdc42 258 worst_latency=current_latency;
labmrd 8:cef07a8bdc42 259 }
labmrd 14:7c5beaa9fb01 260 }
mbed_official 0:8a555873b7d3 261 }
labmrd 2:dfeadd6c651c 262
labmrd 7:b4b95e5f0fce 263
mbed_official 0:8a555873b7d3 264 int main() {
labmrd 2:dfeadd6c651c 265 // Crazy fast baud rate!
labmrd 14:7c5beaa9fb01 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 14:7c5beaa9fb01 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 14:7c5beaa9fb01 302 mx12_left_jaw.Init();
labmrd 14:7c5beaa9fb01 303 //mx12_left_jaw.SetBaud(3000000);
labmrd 14:7c5beaa9fb01 304 //mx12_left_jaw.SetBaud(1000000);
labmrd 14:7c5beaa9fb01 305 //printf("Current Position=%1.3f\n",mx12_left_jaw.GetPosition());
labmrd 14:7c5beaa9fb01 306 mx12_right_jaw.Set_Return_Delay_Time(0.050);
labmrd 14:7c5beaa9fb01 307 printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time());
labmrd 14:7c5beaa9fb01 308 mx12_left_jaw.Set_Return_Delay_Time(0.050);
labmrd 14:7c5beaa9fb01 309 //mx12_left_jaw.Set_Torque_Limit(99.9);
labmrd 14:7c5beaa9fb01 310 //mx12_right_jaw.Set_Torque_Limit(99.9);
labmrd 14:7c5beaa9fb01 311 mx12_left_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
labmrd 14:7c5beaa9fb01 312 mx12_right_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
labmrd 14:7c5beaa9fb01 313 mx12_left_jaw.Set_P_Gain(4);
labmrd 14:7c5beaa9fb01 314 mx12_right_jaw.Set_P_Gain(4);
labmrd 14:7c5beaa9fb01 315 mx12_left_jaw.Set_I_Gain(8);
labmrd 14:7c5beaa9fb01 316 mx12_right_jaw.Set_I_Gain(8);
labmrd 14:7c5beaa9fb01 317 mx12_left_jaw.Set_Alarm_Shutdown(0x04);
labmrd 14:7c5beaa9fb01 318 mx12_right_jaw.Set_Alarm_Shutdown(0x04);
labmrd 14:7c5beaa9fb01 319
labmrd 14:7c5beaa9fb01 320 mx12_left_jaw.Dump_OD_to_Serial(pc);
labmrd 14:7c5beaa9fb01 321 mx12_right_jaw.Dump_OD_to_Serial(pc);
labmrd 14:7c5beaa9fb01 322
labmrd 14:7c5beaa9fb01 323
labmrd 14:7c5beaa9fb01 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 9:f26a91863e3a 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 14:7c5beaa9fb01 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 14:7c5beaa9fb01 385 collect_data=true;
labmrd 2:dfeadd6c651c 386 }
labmrd 2:dfeadd6c651c 387
labmrd 14:7c5beaa9fb01 388 if( collect_data && g_current_cycle >= g_num_cycles)
labmrd 2:dfeadd6c651c 389 {
labmrd 2:dfeadd6c651c 390 // STOOOOOOOOOP
labmrd 14:7c5beaa9fb01 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 14:7c5beaa9fb01 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 14:7c5beaa9fb01 411 if(AuxSerialTimer.read_ms()>100 && collect_data){
labmrd 2:dfeadd6c651c 412 //Send some extra data for GUI purposes
labmrd 14:7c5beaa9fb01 413 printf("<%d,%d,%d,%d,%d,%d,%d> ",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos,
labmrd 14:7c5beaa9fb01 414 tempSpindleData.myServoData[LEFT_SERVO_INDEX].force,
labmrd 14:7c5beaa9fb01 415 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos,
labmrd 14:7c5beaa9fb01 416 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force,
labmrd 14:7c5beaa9fb01 417 tempSpindleData.time,
labmrd 14:7c5beaa9fb01 418 tempSpindleData.direction,
labmrd 14:7c5beaa9fb01 419 tempSpindleData.cycle);
labmrd 14:7c5beaa9fb01 420 printf(" %dus\n", worst_latency);
labmrd 14:7c5beaa9fb01 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 }