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@9:f26a91863e3a, 2015-05-12 (annotated)

Committer:
labmrd
Date:
Tue May 12 17:04:31 2015 +0000
Revision:
9:f26a91863e3a
Parent:
8:cef07a8bdc42
Child:
10:5c0c9c647090
Rod and Trevor don't need John anymore. We are strong independent women who can code by ourselves. (We fixed calibration completely, added a CALIBRATE_TIME_NOW mode, calculate first touch of tissue to trigger end of grasp, and some other minor stuff)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
labmrd 9:f26a91863e3a 1 // Different modes to select
labmrd 2:dfeadd6c651c 2 #define USE_DYNAMIXELS
labmrd 2:dfeadd6c651c 3 //#define USE_BLUETOOTH
labmrd 2:dfeadd6c651c 4 #define USE_SD_CARD
labmrd 5:72e92c721cd5 5 #define ROD_IS_RIGHT
labmrd 9:f26a91863e3a 6 //#define CALIBRATE_TIME_NOW
labmrd 2:dfeadd6c651c 7
labmrd 2:dfeadd6c651c 8 // We have different modes for different things
labmrd 2:dfeadd6c651c 9 #define MODE_MANUAL 1
labmrd 2:dfeadd6c651c 10 #define MODE_AUTOMATIC 2
labmrd 2:dfeadd6c651c 11 #define MODE_IDLE 3
labmrd 2:dfeadd6c651c 12 #define MODE_NULL 0
labmrd 2:dfeadd6c651c 13
labmrd 2:dfeadd6c651c 14 // We always want to know if we are closing or opening
labmrd 2:dfeadd6c651c 15 #define DIRECTION_CLOSING 1
labmrd 2:dfeadd6c651c 16 #define DIRECTION_OPENING 2
labmrd 2:dfeadd6c651c 17 #define DIRECTION_SLACK_WATER 3
labmrd 2:dfeadd6c651c 18 #define DIRECTION_NULL 0
labmrd 2:dfeadd6c651c 19
labmrd 2:dfeadd6c651c 20 // General includes
mbed_official 0:8a555873b7d3 21 #include "mbed.h"
labmrd 2:dfeadd6c651c 22 #include "ServoRingBuffer.h"
labmrd 2:dfeadd6c651c 23 #include "ram_test.h"
labmrd 2:dfeadd6c651c 24 #include "Serial_Receive.h"
labmrd 5:72e92c721cd5 25 #include "data_set.h"
labmrd 2:dfeadd6c651c 26 #include <string>
labmrd 2:dfeadd6c651c 27
labmrd 2:dfeadd6c651c 28 // Specific to Dynamixels
labmrd 2:dfeadd6c651c 29 #ifdef USE_DYNAMIXELS
labmrd 4:e44ac08027bd 30 #include "AD7730.h"
labmrd 2:dfeadd6c651c 31 #endif
labmrd 2:dfeadd6c651c 32
labmrd 2:dfeadd6c651c 33 // Specific to SD Card
labmrd 2:dfeadd6c651c 34 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 35 #include "SDFileSystem.h"
labmrd 2:dfeadd6c651c 36 #endif
labmrd 2:dfeadd6c651c 37
labmrd 2:dfeadd6c651c 38 // Everyone should know pi...
labmrd 2:dfeadd6c651c 39 #ifndef M_PI
labmrd 2:dfeadd6c651c 40 #define M_PI 3.14159265358979323846 /* pi */
labmrd 2:dfeadd6c651c 41 #endif
labmrd 2:dfeadd6c651c 42 #ifndef M_PI_2
labmrd 2:dfeadd6c651c 43 #define M_PI_2 1.57079632679489661923 /* pi/2 */
labmrd 2:dfeadd6c651c 44 #endif
labmrd 2:dfeadd6c651c 45
labmrd 4:e44ac08027bd 46 // Create enum for the Jaw state (Closing, hold, opening)
labmrd 4:e44ac08027bd 47 enum jaw_state{
labmrd 4:e44ac08027bd 48 STATE_CLOSING=0,
labmrd 4:e44ac08027bd 49 STATE_CLOSE_HOLD=1,
labmrd 4:e44ac08027bd 50 STATE_OPENING=2,
labmrd 4:e44ac08027bd 51 STATE_OPEN_HOLD=3
labmrd 4:e44ac08027bd 52 };
labmrd 2:dfeadd6c651c 53
labmrd 2:dfeadd6c651c 54 // Define pins and interrupts
labmrd 2:dfeadd6c651c 55 Ticker potISR; //Define a recurring timer interrupt
labmrd 2:dfeadd6c651c 56 DigitalOut led1(LED1); //Led 1 for debugging purposes
labmrd 2:dfeadd6c651c 57 DigitalOut led2(LED2); //Led 2 for debugging purposes
labmrd 2:dfeadd6c651c 58 DigitalOut led3(LED3); //Led 3 for debugging purposes
labmrd 5:72e92c721cd5 59 DigitalOut led4(LED4); //Led 4 for debugging purposes
labmrd 2:dfeadd6c651c 60 DigitalOut triggerOut(p11);
labmrd 2:dfeadd6c651c 61 Serial pc(USBTX, USBRX); //Set up serial connection to pc
labmrd 9:f26a91863e3a 62 AnalogIn AinLeftForce(p16); //Set up potentiometer on pin 20
labmrd 9:f26a91863e3a 63 AnalogIn AinRightForce(p15); //Set up potentiometer on pin 20
labmrd 9:f26a91863e3a 64
labmrd 9:f26a91863e3a 65 // Specific to bluetooth
labmrd 2:dfeadd6c651c 66 #ifdef USE_BLUETOOTH
labmrd 2:dfeadd6c651c 67 Serial bt(p13,p14); //Set up serial connection to bluetooth adapter
labmrd 2:dfeadd6c651c 68 #endif
labmrd 2:dfeadd6c651c 69
labmrd 9:f26a91863e3a 70 // Specific to SD Card
labmrd 2:dfeadd6c651c 71 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 72 // Attach SD card
labmrd 2:dfeadd6c651c 73 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
labmrd 2:dfeadd6c651c 74 FILE *fp = NULL;
labmrd 2:dfeadd6c651c 75 #define SAMPLES_PER_FILE 10000
labmrd 2:dfeadd6c651c 76 #endif
labmrd 2:dfeadd6c651c 77
labmrd 2:dfeadd6c651c 78 // Dummy variable for debugging
labmrd 2:dfeadd6c651c 79 unsigned int global_count=0;
labmrd 2:dfeadd6c651c 80 float max_percent_full=0;
labmrd 2:dfeadd6c651c 81
labmrd 2:dfeadd6c651c 82 // Define variables for the program
labmrd 2:dfeadd6c651c 83 char g_tissue_type_name[32];
labmrd 2:dfeadd6c651c 84 float g_frequency;
labmrd 2:dfeadd6c651c 85 int g_max_force;
labmrd 2:dfeadd6c651c 86 int g_num_cycles;
labmrd 2:dfeadd6c651c 87 float g_current_trajectory_time;
labmrd 2:dfeadd6c651c 88 float g_theta;
labmrd 2:dfeadd6c651c 89 float g_theta_last=0;
labmrd 2:dfeadd6c651c 90 unsigned char g_current_mode=MODE_NULL;
labmrd 4:e44ac08027bd 91 jaw_state g_current_direction=STATE_OPEN_HOLD;
labmrd 2:dfeadd6c651c 92 unsigned char g_current_cycle=0;
labmrd 5:72e92c721cd5 93 int g_input_pot_1;
labmrd 5:72e92c721cd5 94 #define NUMBER_OF_TISSUES 2
labmrd 5:72e92c721cd5 95 float g_error_norm[NUMBER_OF_TISSUES];
labmrd 5:72e92c721cd5 96 bool g_we_are_grasping=false;
labmrd 9:f26a91863e3a 97 #define CALIBRATION_READS 100.0
labmrd 8:cef07a8bdc42 98 float g_calibration_offset = 8600000.0;
labmrd 5:72e92c721cd5 99
labmrd 9:f26a91863e3a 100 // Values at first touch
labmrd 9:f26a91863e3a 101 int g_masterPositionFirstTouch = 2400;
labmrd 9:f26a91863e3a 102 int g_slavePositionFirstTouch = 2400;
labmrd 9:f26a91863e3a 103 float g_forceAboveFirstTouch = 30000.0;
labmrd 9:f26a91863e3a 104 float g_timeAtFirstTouch;
labmrd 5:72e92c721cd5 105
labmrd 5:72e92c721cd5 106 ///////////Magic numbers courtesy Rod///////////////
labmrd 5:72e92c721cd5 107 float Phi1[5]={-8.02086003501975e-08,
labmrd 5:72e92c721cd5 108 1.55286905458007e-05,
labmrd 5:72e92c721cd5 109 0.00795344249784777,
labmrd 5:72e92c721cd5 110 8.23733045077119,
labmrd 5:72e92c721cd5 111 -0.00299236282304311};
labmrd 5:72e92c721cd5 112
labmrd 5:72e92c721cd5 113 float Phi2[5]={-1.24436831310503e-08,
labmrd 5:72e92c721cd5 114 1.23673348605010e-05,
labmrd 5:72e92c721cd5 115 0.00652545188345528,
labmrd 5:72e92c721cd5 116 6.75893262890734,
labmrd 5:72e92c721cd5 117 -0.00228098997419065};
labmrd 5:72e92c721cd5 118
labmrd 9:f26a91863e3a 119 // Magic numbers to determine if we are grasping (CHANGE TO A DYNAMIC APPROACH!!!!)
labmrd 5:72e92c721cd5 120 float entry_threshold=8.70e6;
labmrd 5:72e92c721cd5 121 float velocty_threshold=-0.01;
labmrd 5:72e92c721cd5 122
labmrd 5:72e92c721cd5 123 float g_thresh_force[NUMBER_OF_TISSUES]={8.80e6,8.75e6};
labmrd 5:72e92c721cd5 124 ///////////Magic numbers courtesy Rod///////////////
labmrd 5:72e92c721cd5 125
labmrd 5:72e92c721cd5 126 int g_command_corrected;
labmrd 5:72e92c721cd5 127
labmrd 8:cef07a8bdc42 128 // These are for load cell initialization of the offset
labmrd 9:f26a91863e3a 129 float g_loadCellZero = 8600000.0;
labmrd 8:cef07a8bdc42 130 bool calibrateDone = false;
labmrd 8:cef07a8bdc42 131 long calibrateTotal = 8600;
labmrd 8:cef07a8bdc42 132 int calibCntr = 0;
labmrd 8:cef07a8bdc42 133 int g_loadCellOffset = 100000;
labmrd 8:cef07a8bdc42 134 int g_threshOffset1 = 0.2e6;
labmrd 8:cef07a8bdc42 135 int g_threshOffset2 = 0.15e6;
labmrd 9:f26a91863e3a 136 long long loadCellTotal = 0;
labmrd 8:cef07a8bdc42 137
labmrd 5:72e92c721cd5 138 Timer mytimer;
labmrd 2:dfeadd6c651c 139
labmrd 2:dfeadd6c651c 140 // Warning, this buffer is large!
labmrd 2:dfeadd6c651c 141 ServoRingBuffer Buffer;
labmrd 2:dfeadd6c651c 142 spindleData tempSpindleData; //For sending to the buffer
labmrd 5:72e92c721cd5 143 #ifdef ROD_IS_RIGHT
labmrd 5:72e92c721cd5 144 int angle_sum=0;
labmrd 5:72e92c721cd5 145 int angle_count=0;
labmrd 5:72e92c721cd5 146 int angledot_sum=0;
labmrd 5:72e92c721cd5 147 int angledot_count=0;
labmrd 5:72e92c721cd5 148 int last_angle=0;
labmrd 5:72e92c721cd5 149 int angledotdot_sum=0;
labmrd 5:72e92c721cd5 150 int angledotdot_count=0;
labmrd 5:72e92c721cd5 151 int last_angledot=0;
labmrd 5:72e92c721cd5 152 #else
labmrd 5:72e92c721cd5 153 data_set recent_pos(30,"Data","Time (us)","Position (encoder counts)");
labmrd 5:72e92c721cd5 154 float coeffs[4];
labmrd 5:72e92c721cd5 155 bool crunching_data_flag;
labmrd 5:72e92c721cd5 156 #endif
labmrd 5:72e92c721cd5 157
labmrd 5:72e92c721cd5 158 int bits_received;
labmrd 2:dfeadd6c651c 159
labmrd 2:dfeadd6c651c 160 Timer ISRDurationTimer;
labmrd 2:dfeadd6c651c 161 Timer AuxSerialTimer;
labmrd 2:dfeadd6c651c 162 int worst_latency=0;
labmrd 2:dfeadd6c651c 163 int current_latency;
labmrd 2:dfeadd6c651c 164
labmrd 9:f26a91863e3a 165 // Specific to Dynamixels
labmrd 2:dfeadd6c651c 166 #ifdef USE_DYNAMIXELS
labmrd 5:72e92c721cd5 167 Serial cm(p28,p27); //Set up serial connection to OpenCM 9.04
labmrd 5:72e92c721cd5 168 unsigned short left_servo_measured=0;
labmrd 5:72e92c721cd5 169 unsigned short right_servo_measured=0;
labmrd 5:72e92c721cd5 170 char input_buffer[16];
labmrd 5:72e92c721cd5 171 int input_buffer_location=0;
labmrd 5:72e92c721cd5 172
labmrd 5:72e92c721cd5 173 float samplingPeriod = 0.001; //This is the sampling period for the timer interrupt
labmrd 2:dfeadd6c651c 174 #define LEFT_JAW_DYNAMIXEL_ID 3
labmrd 2:dfeadd6c651c 175 #define RIGHT_JAW_DYNAMIXEL_ID 4
labmrd 5:72e92c721cd5 176 // #define CLOSED_SERVO_ANGLE_LEFT 1001 //This is the closed in encoder counts
labmrd 5:72e92c721cd5 177 // #define OPEN_SERVO_ANGLE_LEFT 2663 //This is the open in encoder counts
labmrd 5:72e92c721cd5 178 // #define CLOSED_SERVO_ANGLE_RIGHT 3259 //This is the closed in encoder counts
labmrd 5:72e92c721cd5 179 // #define OPEN_SERVO_ANGLE_RIGHT 1486 //This is the open in encoder counts
labmrd 5:72e92c721cd5 180 #define CLOSED_SERVO_ANGLE_LEFT 1975 //This is the closed in encoder counts
labmrd 5:72e92c721cd5 181 #define OPEN_SERVO_ANGLE_LEFT 2560 //This is the open in encoder counts
labmrd 5:72e92c721cd5 182 #define CLOSED_SERVO_ANGLE_RIGHT 1975 //This is the closed in encoder counts
labmrd 5:72e92c721cd5 183 #define OPEN_SERVO_ANGLE_RIGHT 2560 //This is the open in encoder counts
labmrd 5:72e92c721cd5 184 //AD7730( mosi, miso, sclk, ready, cs)
labmrd 5:72e92c721cd5 185 AD7730 adc(p11, p12, p13, p14, p15);
labmrd 5:72e92c721cd5 186 //AD7730 adc2(p11, p12, p13, p18, p19);
labmrd 2:dfeadd6c651c 187
labmrd 5:72e92c721cd5 188
labmrd 6:b4347c69816a 189 AnalogIn AinJoystickFwdBk(p17); //Set up potentiometer on pin 17
labmrd 6:b4347c69816a 190 AnalogIn AinJoystickLftRt(p16); //Set up potentiometer on pin 16
labmrd 5:72e92c721cd5 191 float JoystickFwdBk_Zero=0.5;
labmrd 5:72e92c721cd5 192 float JoystickLftRt_Zero=0.5;
labmrd 4:e44ac08027bd 193
labmrd 4:e44ac08027bd 194 /// Set these to inputs so that they don't interfere with the serial communication
labmrd 2:dfeadd6c651c 195 DigitalIn nullOut1(p21);
labmrd 2:dfeadd6c651c 196 DigitalIn nullOut2(p22);
labmrd 2:dfeadd6c651c 197 DigitalIn nullOut3(p23);
labmrd 2:dfeadd6c651c 198 DigitalIn nullOut4(p24);
labmrd 5:72e92c721cd5 199 /// This one is in the way of the SMD pads
labmrd 5:72e92c721cd5 200 DigitalIn nullOut5(p20);
labmrd 2:dfeadd6c651c 201 #else
labmrd 2:dfeadd6c651c 202 float samplingPeriod = 0.001; //This is the sampling period for the timer interrupt
labmrd 2:dfeadd6c651c 203 #define SERVO_DEGREE_0 900 //This is the pulse width value for HiTEC-422 in microseconds to turn 0 degrees
labmrd 2:dfeadd6c651c 204 #define SERVO_DEGREE_180 2100 //This is the pulse width value for HiTEC-422 in microseconds to turn 180 degrees
labmrd 2:dfeadd6c651c 205 #define MIN_SERVO_ANGLE 0.0 //This is the minimum servo angle in degrees
labmrd 2:dfeadd6c651c 206 #define MAX_SERVO_ANGLE 180.0 //This is the maximum servo angle in degrees
labmrd 2:dfeadd6c651c 207 #define MIN_SERVO_ANGLE_Da_VINCI 20.0 //This is the minimum servo angle in degrees
labmrd 2:dfeadd6c651c 208 #define MAX_SERVO_ANGLE_Da_VINCI 100.0 //This is the maximum servo angle in degrees
labmrd 2:dfeadd6c651c 209 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 210 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 211
labmrd 2:dfeadd6c651c 212 PwmOut myServoLeft(p21); //Set up servo on pin 21
labmrd 2:dfeadd6c651c 213 PwmOut myServoRight(p22); //Set up servo on pin 22
labmrd 2:dfeadd6c651c 214 AnalogIn AinLeftPosition(p20); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 215 AnalogIn AinRightPosition(p19); //Set up potentiometer on pin 20
labmrd 2:dfeadd6c651c 216
labmrd 2:dfeadd6c651c 217
labmrd 2:dfeadd6c651c 218 // Function moveServoTo: Convert a degree value to pulsewidth for Servo
labmrd 2:dfeadd6c651c 219 void moveServoTo(float angle) {
labmrd 2:dfeadd6c651c 220 // Make sure none of the user input falls outside of min and max angle limits
labmrd 2:dfeadd6c651c 221 if( angle < MIN_SERVO_ANGLE){angle = MIN_SERVO_ANGLE;}
labmrd 2:dfeadd6c651c 222 else if(angle > MAX_SERVO_ANGLE){angle = MAX_SERVO_ANGLE;}
labmrd 2:dfeadd6c651c 223 myServoLeft.pulsewidth(servoOffset + servoConversion*(180-angle));
labmrd 2:dfeadd6c651c 224 myServoRight.pulsewidth(servoOffset + servoConversion*(angle));
labmrd 2:dfeadd6c651c 225 }
labmrd 2:dfeadd6c651c 226
labmrd 2:dfeadd6c651c 227 #endif
labmrd 2:dfeadd6c651c 228
labmrd 9:f26a91863e3a 229 // Serial interrupt function
labmrd 5:72e92c721cd5 230 void serialInterrupt(char buffer){
labmrd 5:72e92c721cd5 231 input_buffer[input_buffer_location]=buffer;
labmrd 5:72e92c721cd5 232 input_buffer_location++;
labmrd 5:72e92c721cd5 233 bits_received+=8;
labmrd 5:72e92c721cd5 234 //pc.printf("RC:%d\n",buffer);
labmrd 5:72e92c721cd5 235
labmrd 5:72e92c721cd5 236
labmrd 5:72e92c721cd5 237 //Is the packet looking good so far??
labmrd 5:72e92c721cd5 238 if(input_buffer[0]==0xFF){
labmrd 5:72e92c721cd5 239 //Is the packet looking good so far??????
labmrd 5:72e92c721cd5 240 if(input_buffer[1]==0xFF || input_buffer_location ==1){
labmrd 5:72e92c721cd5 241 //Do we have a complete packet??
labmrd 5:72e92c721cd5 242 if(input_buffer_location>=6){ //This is 6 because we already incremented
labmrd 9:f26a91863e3a 243 //We do! Extract the juicy datas
labmrd 5:72e92c721cd5 244 left_servo_measured = ( input_buffer[2] << 8 ) | input_buffer[3];
labmrd 5:72e92c721cd5 245 right_servo_measured = ( input_buffer[4] << 8 ) | input_buffer[5];
labmrd 5:72e92c721cd5 246 //pc.printf("RP:%d,%d\n",left_servo_measured,right_servo_measured);
labmrd 5:72e92c721cd5 247 //Reset the buffer location so we can start over
labmrd 5:72e92c721cd5 248 input_buffer_location=0;
labmrd 5:72e92c721cd5 249 }
labmrd 5:72e92c721cd5 250 }else{
labmrd 5:72e92c721cd5 251 //Something is wrong. We may just not be at the correct location in the packet
labmrd 5:72e92c721cd5 252 //Reset the buffer location so we can start over
labmrd 5:72e92c721cd5 253 input_buffer_location=0;
labmrd 5:72e92c721cd5 254 //printf("Error, Byte 2 not what was expected: 0xFF!=0x%02x\n",input_buffer[1]);
labmrd 5:72e92c721cd5 255 }
labmrd 5:72e92c721cd5 256 }else{
labmrd 5:72e92c721cd5 257 //Something is wrong. We may just not be at the correct location in the packet
labmrd 5:72e92c721cd5 258 //Reset the buffer location so we can start over
labmrd 5:72e92c721cd5 259 input_buffer_location=0;
labmrd 5:72e92c721cd5 260 //printf("Error, Byte 1 not what was expected: 0xFF!=0x%02x\n",input_buffer[0]);
labmrd 5:72e92c721cd5 261 }
labmrd 5:72e92c721cd5 262 }
labmrd 5:72e92c721cd5 263
labmrd 5:72e92c721cd5 264
labmrd 4:e44ac08027bd 265 // 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 266 float trapezoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) {
labmrd 4:e44ac08027bd 267 // Define variables specific to this function
labmrd 4:e44ac08027bd 268 float y_trapezoid = 0.0;
labmrd 4:e44ac08027bd 269 float timeMod;
labmrd 4:e44ac08027bd 270 float modifiedFrequency = g_frequency/2.0;
labmrd 4:e44ac08027bd 271 float period = 1/modifiedFrequency;
labmrd 4:e44ac08027bd 272 cycle_num=t*modifiedFrequency;
labmrd 4:e44ac08027bd 273
labmrd 4:e44ac08027bd 274 // Take the time and mod it with the period to be able to break up each cycle into 4 piecewise sections
labmrd 4:e44ac08027bd 275 timeMod = fmodf(t,period);
labmrd 4:e44ac08027bd 276
labmrd 9:f26a91863e3a 277 // Determining trajectory
labmrd 4:e44ac08027bd 278 if (timeMod < period/4.0)
labmrd 4:e44ac08027bd 279 {
labmrd 4:e44ac08027bd 280 y_trapezoid = (-4.0/period)*(timeMod)+1.0;
labmrd 4:e44ac08027bd 281 state = STATE_CLOSING;
labmrd 4:e44ac08027bd 282 }
labmrd 4:e44ac08027bd 283 else if (timeMod >= period/4.0 && timeMod < period/2.0)
labmrd 4:e44ac08027bd 284 {
labmrd 4:e44ac08027bd 285 y_trapezoid = 0.0;
labmrd 4:e44ac08027bd 286 state = STATE_CLOSE_HOLD;
labmrd 4:e44ac08027bd 287 }
labmrd 4:e44ac08027bd 288 else if (timeMod >= period/2.0 && timeMod < 3*period/4.0)
labmrd 4:e44ac08027bd 289 {
labmrd 4:e44ac08027bd 290 y_trapezoid = (4.0/period)*(timeMod)-2;
labmrd 4:e44ac08027bd 291 state = STATE_OPENING;
labmrd 4:e44ac08027bd 292 }
labmrd 4:e44ac08027bd 293 else if (timeMod >= 3*period/4.0)
labmrd 4:e44ac08027bd 294 {
labmrd 4:e44ac08027bd 295 y_trapezoid = 1.0;
labmrd 4:e44ac08027bd 296 state = STATE_OPEN_HOLD;
labmrd 4:e44ac08027bd 297 }
labmrd 4:e44ac08027bd 298
labmrd 4:e44ac08027bd 299 return y_trapezoid;
labmrd 4:e44ac08027bd 300 }
labmrd 4:e44ac08027bd 301
labmrd 4:e44ac08027bd 302 void sinusoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) {
labmrd 4:e44ac08027bd 303 //Fill me with SCIENCE!!!
labmrd 4:e44ac08027bd 304 }
labmrd 4:e44ac08027bd 305
labmrd 4:e44ac08027bd 306
labmrd 2:dfeadd6c651c 307 // Function timerISRFunction: Timer ISR function to collect data and write to ring buffer
labmrd 2:dfeadd6c651c 308 void timerISRFunction() {
labmrd 9:f26a91863e3a 309 //if (calibrateDone){
labmrd 8:cef07a8bdc42 310 //led 1 is used as a 'thinking' light, brighter=worse
labmrd 8:cef07a8bdc42 311 led1 = 1;
labmrd 8:cef07a8bdc42 312 led2 = 0;
labmrd 8:cef07a8bdc42 313 triggerOut = 1;
labmrd 2:dfeadd6c651c 314
labmrd 8:cef07a8bdc42 315 ISRDurationTimer.reset();
labmrd 8:cef07a8bdc42 316 ISRDurationTimer.start();
labmrd 8:cef07a8bdc42 317 if(g_current_mode==MODE_AUTOMATIC){
labmrd 8:cef07a8bdc42 318
labmrd 8:cef07a8bdc42 319 // Warning, this calculation is in the ISR and as such is probably slower than we would prefer.
labmrd 8:cef07a8bdc42 320 // @todo The math could certainly be optimized with some precalculated constants. Lookup tables are faster than sin()
labmrd 8:cef07a8bdc42 321 float percent=trapezoidalTrajectory(g_current_trajectory_time,g_current_direction,g_current_cycle);
labmrd 8:cef07a8bdc42 322 g_current_trajectory_time+=samplingPeriod;
labmrd 8:cef07a8bdc42 323
labmrd 8:cef07a8bdc42 324
labmrd 8:cef07a8bdc42 325 //float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
labmrd 8:cef07a8bdc42 326 //g_current_direction=(cos(angle)<0);
labmrd 8:cef07a8bdc42 327 //g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
labmrd 8:cef07a8bdc42 328
labmrd 8:cef07a8bdc42 329
labmrd 8:cef07a8bdc42 330 #ifdef USE_DYNAMIXELS
labmrd 8:cef07a8bdc42 331 //float percent=(sin(angle)+1)/2.0;
labmrd 8:cef07a8bdc42 332 if(adc.isReady()){
labmrd 8:cef07a8bdc42 333 adc.interruptRead();
labmrd 8:cef07a8bdc42 334 }
labmrd 8:cef07a8bdc42 335
labmrd 8:cef07a8bdc42 336 while(cm.readable()){
labmrd 8:cef07a8bdc42 337 led4=1;
labmrd 8:cef07a8bdc42 338 serialInterrupt(cm.getc());
labmrd 8:cef07a8bdc42 339 }
labmrd 8:cef07a8bdc42 340 led4=0;
labmrd 8:cef07a8bdc42 341
labmrd 8:cef07a8bdc42 342 short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
labmrd 8:cef07a8bdc42 343 short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
labmrd 8:cef07a8bdc42 344
labmrd 8:cef07a8bdc42 345 //Send the commanded position to the OpenCM board
labmrd 8:cef07a8bdc42 346 cm.putc(0xFF);
labmrd 8:cef07a8bdc42 347 cm.putc(0xFF);
labmrd 8:cef07a8bdc42 348 cm.putc(left_servo >> 8); //Top 4 bits
labmrd 8:cef07a8bdc42 349 cm.putc(left_servo & 0xFF); //Bottom 8 bits
labmrd 8:cef07a8bdc42 350 cm.putc(right_servo >> 8); //Top 4 bits
labmrd 8:cef07a8bdc42 351 cm.putc(right_servo & 0xFF); //Bottom 8 bits
labmrd 8:cef07a8bdc42 352
labmrd 8:cef07a8bdc42 353 tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = adc.read();
labmrd 8:cef07a8bdc42 354 tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = left_servo_measured;
labmrd 8:cef07a8bdc42 355 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = 0;
labmrd 8:cef07a8bdc42 356 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = right_servo_measured;
labmrd 8:cef07a8bdc42 357 tempSpindleData.direction=g_current_direction;
labmrd 8:cef07a8bdc42 358 tempSpindleData.cycle=g_current_cycle;
labmrd 8:cef07a8bdc42 359 Buffer.write(tempSpindleData);
labmrd 8:cef07a8bdc42 360 #else
labmrd 8:cef07a8bdc42 361 g_theta=(1.0-percent)*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI;
labmrd 8:cef07a8bdc42 362 tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = AinLeftForce.read_u16();
labmrd 8:cef07a8bdc42 363 tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = AinLeftPosition.read_u16();
labmrd 8:cef07a8bdc42 364 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
labmrd 8:cef07a8bdc42 365 tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = AinRightPosition.read_u16();
labmrd 8:cef07a8bdc42 366 tempSpindleData.direction=g_current_direction;
labmrd 8:cef07a8bdc42 367 tempSpindleData.cycle=g_current_cycle;
labmrd 8:cef07a8bdc42 368 Buffer.write(tempSpindleData);
labmrd 8:cef07a8bdc42 369
labmrd 8:cef07a8bdc42 370
labmrd 8:cef07a8bdc42 371
labmrd 8:cef07a8bdc42 372 moveServoTo(g_theta); // in degrees, son
labmrd 8:cef07a8bdc42 373 #endif
labmrd 8:cef07a8bdc42 374 }else if(g_current_mode==MODE_MANUAL){
labmrd 8:cef07a8bdc42 375
labmrd 8:cef07a8bdc42 376 g_current_trajectory_time+=samplingPeriod;
labmrd 4:e44ac08027bd 377 if(adc.isReady()){
labmrd 4:e44ac08027bd 378 adc.interruptRead();
labmrd 4:e44ac08027bd 379 }
labmrd 4:e44ac08027bd 380
labmrd 8:cef07a8bdc42 381 int im_tired_of_this_game=0;
labmrd 8:cef07a8bdc42 382 while(cm.readable() && im_tired_of_this_game++<100){
labmrd 5:72e92c721cd5 383 serialInterrupt(cm.getc());
labmrd 5:72e92c721cd5 384 }
labmrd 8:cef07a8bdc42 385
labmrd 8:cef07a8bdc42 386 // float pot_open=0.75;
labmrd 8:cef07a8bdc42 387 // float pot_closed=0.48;
labmrd 8:cef07a8bdc42 388 //
labmrd 8:cef07a8bdc42 389 // float percent=AinJoystickFwdBk.read();
labmrd 8:cef07a8bdc42 390 // percent=(pot_open-percent)/(pot_open-pot_closed);
labmrd 8:cef07a8bdc42 391 // float skew =0;//AinJoystickLftRt.read()-JoystickLftRt_Zero;
labmrd 8:cef07a8bdc42 392 //
labmrd 8:cef07a8bdc42 393 // // The 'pulling' of the trigger corresponds to open/closed
labmrd 8:cef07a8bdc42 394 // short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
labmrd 8:cef07a8bdc42 395 // short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
labmrd 8:cef07a8bdc42 396 //
labmrd 8:cef07a8bdc42 397 // g_input_pot_1=left_servo;
labmrd 8:cef07a8bdc42 398 short left_servo =g_command_corrected;
labmrd 8:cef07a8bdc42 399 short right_servo=g_command_corrected;
labmrd 5:72e92c721cd5 400
labmrd 8:cef07a8bdc42 401 // // The 'skewing' of the trigger corresponds to yawing the jaws left or right, while maintaining open/closed
labmrd 8:cef07a8bdc42 402 // left_servo +=skew*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT );
labmrd 8:cef07a8bdc42 403 // right_servo-=skew*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT);
labmrd 8:cef07a8bdc42 404 //
labmrd 8:cef07a8bdc42 405 // /// Todo: There is undoubtedly a cleaner way to do this.
labmrd 8:cef07a8bdc42 406 // if(OPEN_SERVO_ANGLE_LEFT < CLOSED_SERVO_ANGLE_LEFT && left_servo < OPEN_SERVO_ANGLE_LEFT ){left_servo =OPEN_SERVO_ANGLE_LEFT; }
labmrd 8:cef07a8bdc42 407 // if(OPEN_SERVO_ANGLE_LEFT > CLOSED_SERVO_ANGLE_LEFT && left_servo > OPEN_SERVO_ANGLE_LEFT ){left_servo =OPEN_SERVO_ANGLE_LEFT; }
labmrd 8:cef07a8bdc42 408 // if(OPEN_SERVO_ANGLE_RIGHT < CLOSED_SERVO_ANGLE_RIGHT && right_servo < OPEN_SERVO_ANGLE_RIGHT){right_servo=OPEN_SERVO_ANGLE_RIGHT;}
labmrd 8:cef07a8bdc42 409 // if(OPEN_SERVO_ANGLE_RIGHT > CLOSED_SERVO_ANGLE_RIGHT && right_servo > OPEN_SERVO_ANGLE_RIGHT){right_servo=OPEN_SERVO_ANGLE_RIGHT;}
labmrd 8:cef07a8bdc42 410 //
labmrd 8:cef07a8bdc42 411 // // Just to make sure
labmrd 8:cef07a8bdc42 412 // // left_servo=(left_servo+4096)%4096;
labmrd 8:cef07a8bdc42 413 // if(left_servo < 0){left_servo = 0;}
labmrd 8:cef07a8bdc42 414 // if(left_servo >4095){left_servo =4095;}
labmrd 8:cef07a8bdc42 415 // if(right_servo< 0){right_servo= 0;}
labmrd 8:cef07a8bdc42 416 // if(right_servo>4095){right_servo=4095;}
labmrd 4:e44ac08027bd 417
labmrd 5:72e92c721cd5 418 //Send the commanded position to the OpenCM board
labmrd 5:72e92c721cd5 419 cm.putc(0xFF);
labmrd 5:72e92c721cd5 420 cm.putc(0xFF);
labmrd 5:72e92c721cd5 421 cm.putc(left_servo >> 8); //Top 4 bits
labmrd 5:72e92c721cd5 422 cm.putc(left_servo & 0xFF); //Bottom 8 bits
labmrd 5:72e92c721cd5 423 cm.putc(right_servo >> 8); //Top 4 bits
labmrd 5:72e92c721cd5 424 cm.putc(right_servo & 0xFF); //Bottom 8 bits
labmrd 5:72e92c721cd5 425
labmrd 8:cef07a8bdc42 426 #ifdef ROD_IS_RIGHT
labmrd 8:cef07a8bdc42 427 //Famous Rod's Magical World of Good Ideas Part 1
labmrd 8:cef07a8bdc42 428 int angle=left_servo_measured;
labmrd 8:cef07a8bdc42 429 angle_sum+=angle;
labmrd 8:cef07a8bdc42 430 if(angle_count!=0){
labmrd 8:cef07a8bdc42 431 int angledot=(angle-last_angle);
labmrd 8:cef07a8bdc42 432 angledot_sum+=angledot;
labmrd 8:cef07a8bdc42 433 if(angledot_count!=0){
labmrd 8:cef07a8bdc42 434 int angledotdot=(angledot-last_angledot);
labmrd 8:cef07a8bdc42 435 angledotdot_sum+=angledotdot;
labmrd 8:cef07a8bdc42 436 angledotdot_count++;
labmrd 8:cef07a8bdc42 437 }
labmrd 8:cef07a8bdc42 438 angledot_count++;
labmrd 8:cef07a8bdc42 439 last_angledot=angledot;
labmrd 8:cef07a8bdc42 440 }
labmrd 8:cef07a8bdc42 441 angle_count++;
labmrd 8:cef07a8bdc42 442 last_angle=angle;
labmrd 8:cef07a8bdc42 443 #else
labmrd 8:cef07a8bdc42 444 // John's worse than Rod ideas of terribleness
labmrd 8:cef07a8bdc42 445 if(!crunching_data_flag){ // Only add data if no one is using it
labmrd 8:cef07a8bdc42 446 recent_pos.add_data(left_servo_measured,g_current_trajectory_time);
labmrd 8:cef07a8bdc42 447 }
labmrd 8:cef07a8bdc42 448 #endif
labmrd 8:cef07a8bdc42 449 }
labmrd 2:dfeadd6c651c 450
labmrd 8:cef07a8bdc42 451 //done thinking
labmrd 8:cef07a8bdc42 452 led1 = 0;
labmrd 8:cef07a8bdc42 453 led2 = 1;
labmrd 8:cef07a8bdc42 454 triggerOut = 0;
labmrd 2:dfeadd6c651c 455
labmrd 8:cef07a8bdc42 456 ISRDurationTimer.stop();
labmrd 8:cef07a8bdc42 457 current_latency=ISRDurationTimer.read_us();
labmrd 8:cef07a8bdc42 458 if(current_latency>worst_latency){
labmrd 8:cef07a8bdc42 459 worst_latency=current_latency;
labmrd 8:cef07a8bdc42 460 }
labmrd 9:f26a91863e3a 461 //}
mbed_official 0:8a555873b7d3 462 }
labmrd 2:dfeadd6c651c 463
labmrd 7:b4b95e5f0fce 464 // Calibrate Load Cell Function
labmrd 7:b4b95e5f0fce 465 void calibrateLoadCell() {
labmrd 8:cef07a8bdc42 466 long long loadCellTotal = 0;
labmrd 9:f26a91863e3a 467 int intermediateValue;
labmrd 7:b4b95e5f0fce 468 for(int ii=0;ii<CALIBRATION_READS;ii++){
labmrd 9:f26a91863e3a 469 intermediateValue = adc.read();
labmrd 9:f26a91863e3a 470 //pc.printf("Load Cell Read: %i\n",intermediateValue);
labmrd 9:f26a91863e3a 471 loadCellTotal+= intermediateValue;
labmrd 8:cef07a8bdc42 472 wait_ms(2);
labmrd 9:f26a91863e3a 473 pc.printf("We are calibrating load cells...\n");
labmrd 7:b4b95e5f0fce 474 }
labmrd 9:f26a91863e3a 475 g_calibration_offset=float(loadCellTotal)/float(CALIBRATION_READS);
labmrd 8:cef07a8bdc42 476 //Update values for thresholding based on calibration
labmrd 8:cef07a8bdc42 477 g_thresh_force[0]=g_threshOffset1+g_calibration_offset;
labmrd 8:cef07a8bdc42 478 g_thresh_force[1]=g_threshOffset2+g_calibration_offset;
labmrd 8:cef07a8bdc42 479 calibrateDone = true;
labmrd 7:b4b95e5f0fce 480 }
labmrd 7:b4b95e5f0fce 481
mbed_official 0:8a555873b7d3 482 int main() {
labmrd 2:dfeadd6c651c 483 // Crazy fast baud rate!
labmrd 5:72e92c721cd5 484 pc.baud(115200);
labmrd 5:72e92c721cd5 485
labmrd 5:72e92c721cd5 486 // For communication with OpenCM 9.04 Board
labmrd 5:72e92c721cd5 487 cm.baud(1000000);
labmrd 2:dfeadd6c651c 488
labmrd 2:dfeadd6c651c 489 #ifdef USE_BLUETOOTH
labmrd 2:dfeadd6c651c 490 bt.baud(9600);
labmrd 2:dfeadd6c651c 491 #endif
labmrd 2:dfeadd6c651c 492
labmrd 2:dfeadd6c651c 493 // Attach ISR routines
labmrd 8:cef07a8bdc42 494 potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod
labmrd 8:cef07a8bdc42 495
labmrd 2:dfeadd6c651c 496
labmrd 2:dfeadd6c651c 497 // Some debug info:
labmrd 2:dfeadd6c651c 498 //DisplayRAMBanks();
labmrd 2:dfeadd6c651c 499 //printf ("System clock = %d\r\n", SystemCoreClock);
labmrd 2:dfeadd6c651c 500
labmrd 2:dfeadd6c651c 501 pc.printf("\n\n\n");
labmrd 2:dfeadd6c651c 502 pc.printf("----------------------------------\n");
labmrd 2:dfeadd6c651c 503 pc.printf("| |\n");
labmrd 2:dfeadd6c651c 504 pc.printf("| Welcome to our mbed! |\n");
labmrd 2:dfeadd6c651c 505 pc.printf("| |\n");
labmrd 2:dfeadd6c651c 506 pc.printf("| John and Trevor, Proprietors |\n");
labmrd 2:dfeadd6c651c 507 pc.printf("| |\n");
labmrd 2:dfeadd6c651c 508 pc.printf("----------------------------------\n");
labmrd 2:dfeadd6c651c 509 pc.printf(" ||\n");
labmrd 2:dfeadd6c651c 510 pc.printf(" ||\n");
labmrd 2:dfeadd6c651c 511 pc.printf(" || _\n");
labmrd 2:dfeadd6c651c 512 pc.printf(" || _( )_\n");
labmrd 2:dfeadd6c651c 513 pc.printf(" || (_(#)_)\n");
labmrd 2:dfeadd6c651c 514 pc.printf(" || (_)\\\n");
labmrd 2:dfeadd6c651c 515 pc.printf(" || | __\n");
labmrd 2:dfeadd6c651c 516 pc.printf(" \\ / | || |/_/\n");
labmrd 2:dfeadd6c651c 517 pc.printf(" / | | / / / | || | \\ \\ | / \n");
labmrd 2:dfeadd6c651c 518 pc.printf("/ / \\ \\ / / / || / | / / \\ \\ \n");
labmrd 2:dfeadd6c651c 519 pc.printf("#################################\n");
labmrd 2:dfeadd6c651c 520 pc.printf("#################################\n");
labmrd 2:dfeadd6c651c 521 pc.printf("\n\n");
labmrd 2:dfeadd6c651c 522
labmrd 2:dfeadd6c651c 523 #ifdef USE_DYNAMIXELS
labmrd 4:e44ac08027bd 524 adc.setFilter(256 , false, 1);
labmrd 4:e44ac08027bd 525 adc.start();
labmrd 9:f26a91863e3a 526
labmrd 5:72e92c721cd5 527 JoystickLftRt_Zero=0;
labmrd 5:72e92c721cd5 528 int calib_N=100;
labmrd 5:72e92c721cd5 529 for(int ii=0;ii<calib_N;ii++){JoystickLftRt_Zero+=AinJoystickLftRt.read();}
labmrd 5:72e92c721cd5 530 JoystickLftRt_Zero=JoystickLftRt_Zero/calib_N;
labmrd 5:72e92c721cd5 531 g_current_mode=MODE_MANUAL;
labmrd 2:dfeadd6c651c 532 #else
labmrd 2:dfeadd6c651c 533 // Configure Servo for HiTec 422
labmrd 2:dfeadd6c651c 534 myServoLeft.period_ms(20);
labmrd 2:dfeadd6c651c 535 myServoRight.period_ms(20);
labmrd 2:dfeadd6c651c 536 #endif
labmrd 2:dfeadd6c651c 537
labmrd 8:cef07a8bdc42 538
labmrd 7:b4b95e5f0fce 539
labmrd 2:dfeadd6c651c 540 printf("Setup Complete.\n");
labmrd 2:dfeadd6c651c 541 AuxSerialTimer.start();
labmrd 5:72e92c721cd5 542 mytimer.start();
labmrd 9:f26a91863e3a 543
labmrd 9:f26a91863e3a 544
labmrd 9:f26a91863e3a 545 //wait_ms(1000);
labmrd 9:f26a91863e3a 546 //calibrateLoadCell();
labmrd 2:dfeadd6c651c 547 while(1)
labmrd 2:dfeadd6c651c 548 {
labmrd 2:dfeadd6c651c 549 // spin in a main loop. serialISR will interrupt it to call serialPot
labmrd 2:dfeadd6c651c 550
labmrd 8:cef07a8bdc42 551
labmrd 8:cef07a8bdc42 552
labmrd 5:72e92c721cd5 553 #ifdef USE_DYNAMIXELS
labmrd 5:72e92c721cd5 554
labmrd 5:72e92c721cd5 555 #endif
labmrd 5:72e92c721cd5 556
labmrd 2:dfeadd6c651c 557 ///This checks for any new serial bytes, and returns true if
labmrd 2:dfeadd6c651c 558 ///we have an entire packet ready. The new packet will live
labmrd 2:dfeadd6c651c 559 ///in newData.
labmrd 2:dfeadd6c651c 560 if(
labmrd 2:dfeadd6c651c 561 #ifdef USE_BLUETOOTH
labmrd 2:dfeadd6c651c 562 receivePacket(bt)
labmrd 2:dfeadd6c651c 563 #else
labmrd 2:dfeadd6c651c 564 receivePacket(pc)
labmrd 2:dfeadd6c651c 565 #endif
labmrd 2:dfeadd6c651c 566 )
labmrd 2:dfeadd6c651c 567 {
labmrd 2:dfeadd6c651c 568 // < Tissue Type (string), Frequency Value (Hz) (int), Force Max (N) (int), # Cycles (in) >
labmrd 2:dfeadd6c651c 569 //<date/tissue/time,2,3,4>
labmrd 2:dfeadd6c651c 570 //g_tissue_type_name=tissue_type_name;
labmrd 2:dfeadd6c651c 571 std::string file_name_in=inString.substr(0, inString.find(","));
labmrd 2:dfeadd6c651c 572 g_frequency=newData[1]/10.0; // Since all we send are ints
labmrd 2:dfeadd6c651c 573 g_max_force=newData[2];
labmrd 2:dfeadd6c651c 574 g_num_cycles=newData[3];
labmrd 2:dfeadd6c651c 575 g_current_trajectory_time=0;
labmrd 4:e44ac08027bd 576 g_current_cycle=0;
labmrd 2:dfeadd6c651c 577 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 578 int first_slash=file_name_in.find("/");
labmrd 2:dfeadd6c651c 579 std::string new_dir="/sd/"+file_name_in.substr(0, first_slash);
labmrd 2:dfeadd6c651c 580 std::string new_subdir="/sd/"+file_name_in.substr(0, file_name_in.find("/",first_slash+1));
labmrd 2:dfeadd6c651c 581 mkdir(new_dir.c_str(), 0777);
labmrd 2:dfeadd6c651c 582 mkdir(new_subdir.c_str(), 0777);
labmrd 2:dfeadd6c651c 583 std::string file_name="/sd/"+file_name_in+".csv";
labmrd 2:dfeadd6c651c 584 //pc.printf("subdir=\"%s\"\n",file_name.c_str());
labmrd 2:dfeadd6c651c 585 fp = fopen(file_name.c_str(), "w");
labmrd 2:dfeadd6c651c 586 //FILE *fp = fopen("/sd/data/sdtest.txt", "w");
labmrd 2:dfeadd6c651c 587 if(fp == NULL) {
labmrd 2:dfeadd6c651c 588 error("Could not open file for write\n");
labmrd 2:dfeadd6c651c 589 }
labmrd 2:dfeadd6c651c 590 fprintf(fp, "%%Starting New Trajectory\n");
labmrd 2:dfeadd6c651c 591 fprintf(fp, "%%File Name=\"%s\"\n",file_name.c_str());
labmrd 2:dfeadd6c651c 592 fprintf(fp, "%%Current Mode=AUTOMATIC\n");
labmrd 4:e44ac08027bd 593 fprintf(fp, "%%Trajectory Type=TRAPEZOIDAL\n");
labmrd 2:dfeadd6c651c 594 fprintf(fp, "%%Frequency=%f Hz\n",g_frequency);
labmrd 2:dfeadd6c651c 595 fprintf(fp, "%%Max Force=%f ??\n",g_max_force);
labmrd 2:dfeadd6c651c 596 fprintf(fp, "%%Num Cycles=%d\n",g_num_cycles);
labmrd 2:dfeadd6c651c 597 fprintf(fp, "%%Re. Direction: ,Closing=%d,Opening=%d,Undef=%d\n", DIRECTION_CLOSING , DIRECTION_OPENING , DIRECTION_SLACK_WATER );
labmrd 2:dfeadd6c651c 598 fprintf(fp, "%%PositionLeft,ForceLeft,PositionRight,ForceRight,Time(ms),Direction,CycleNum\n");
labmrd 2:dfeadd6c651c 599 #endif
labmrd 2:dfeadd6c651c 600 // We are go-times!
labmrd 5:72e92c721cd5 601 g_current_mode=MODE_AUTOMATIC;
labmrd 2:dfeadd6c651c 602 }
labmrd 2:dfeadd6c651c 603
labmrd 5:72e92c721cd5 604 if( g_current_mode==MODE_AUTOMATIC && g_current_cycle >= g_num_cycles)
labmrd 2:dfeadd6c651c 605 {
labmrd 2:dfeadd6c651c 606 // STOOOOOOOOOP
labmrd 5:72e92c721cd5 607 g_current_mode=MODE_NULL;
labmrd 2:dfeadd6c651c 608 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 609 // Close the file
labmrd 2:dfeadd6c651c 610 fclose(fp);
labmrd 2:dfeadd6c651c 611 fp = NULL;
labmrd 2:dfeadd6c651c 612 #endif
labmrd 2:dfeadd6c651c 613 }
labmrd 2:dfeadd6c651c 614
labmrd 5:72e92c721cd5 615 // This section of code should run whenever there is free time to save datas
labmrd 2:dfeadd6c651c 616 #ifdef USE_SD_CARD
labmrd 2:dfeadd6c651c 617 if(fp != NULL) {
labmrd 2:dfeadd6c651c 618 // Only write to SD if there is a valid file handle
labmrd 2:dfeadd6c651c 619 led3 = 1;
labmrd 2:dfeadd6c651c 620 Buffer.dumpBufferToSD(fp);
labmrd 2:dfeadd6c651c 621 led3 = 0;
labmrd 2:dfeadd6c651c 622 }
labmrd 2:dfeadd6c651c 623 #else
labmrd 5:72e92c721cd5 624 //Warning, this is a big fat stream of data, 1 minute is approx 3MB
labmrd 5:72e92c721cd5 625 //I certainly recommend using the SD card.
labmrd 2:dfeadd6c651c 626 Buffer.dumpBufferToSerial();
labmrd 2:dfeadd6c651c 627 #endif
labmrd 2:dfeadd6c651c 628
labmrd 5:72e92c721cd5 629 if(g_current_mode!=MODE_NULL && AuxSerialTimer.read_us()>30000){
labmrd 2:dfeadd6c651c 630 //Send some extra data for GUI purposes
labmrd 5:72e92c721cd5 631 #ifdef ROD_IS_RIGHT
labmrd 5:72e92c721cd5 632 float angle=0,angledot=0,angledotdot=0;
labmrd 5:72e92c721cd5 633 if(angledotdot_count>0){
labmrd 5:72e92c721cd5 634 angle =float(angle_sum )/angle_count;
labmrd 5:72e92c721cd5 635 angledot =float(angledot_sum )/angledot_count;
labmrd 5:72e92c721cd5 636 angledotdot=float(angledotdot_sum)/angledotdot_count;
labmrd 5:72e92c721cd5 637 }
labmrd 5:72e92c721cd5 638 angle_sum=0;angle_count=0;
labmrd 5:72e92c721cd5 639 angledot_sum=0;angledot_count=0;
labmrd 5:72e92c721cd5 640 angledotdot_sum=0;angledotdot_count=0;
labmrd 5:72e92c721cd5 641 #else
labmrd 5:72e92c721cd5 642 crunching_data_flag=true;
labmrd 5:72e92c721cd5 643 recent_pos.least_squares_3rd(coeffs);
labmrd 5:72e92c721cd5 644 crunching_data_flag=false;
labmrd 5:72e92c721cd5 645 float x=g_current_trajectory_time;
labmrd 5:72e92c721cd5 646 int angle =coeffs[0]+coeffs[1]*x+coeffs[2]*x*x+coeffs[3]*x*x*x;
labmrd 5:72e92c721cd5 647 int angledot = coeffs[1]+2*coeffs[2]*x+3*coeffs[3]*x*x ;
labmrd 5:72e92c721cd5 648 int angledotdot= 2*coeffs[2] +6*coeffs[3]*x ;
labmrd 5:72e92c721cd5 649 #endif
labmrd 5:72e92c721cd5 650
labmrd 5:72e92c721cd5 651
labmrd 5:72e92c721cd5 652
labmrd 5:72e92c721cd5 653 float pot_open=0.75;
labmrd 5:72e92c721cd5 654 float pot_closed=0.48;
labmrd 5:72e92c721cd5 655
labmrd 5:72e92c721cd5 656 float percent=0.0;
labmrd 5:72e92c721cd5 657 //Average the pot a bunch, it's NOISY
labmrd 5:72e92c721cd5 658 for(int i=0;i<100;i++){
labmrd 5:72e92c721cd5 659 percent+=AinJoystickFwdBk.read();
labmrd 5:72e92c721cd5 660 }
labmrd 5:72e92c721cd5 661 percent=percent/100.0;
labmrd 5:72e92c721cd5 662 percent=(pot_open-percent)/(pot_open-pot_closed);
labmrd 5:72e92c721cd5 663
labmrd 5:72e92c721cd5 664 // The 'pulling' of the trigger corresponds to open/closed
labmrd 9:f26a91863e3a 665 int potread = percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
labmrd 5:72e92c721cd5 666
labmrd 5:72e92c721cd5 667
labmrd 5:72e92c721cd5 668
labmrd 5:72e92c721cd5 669
labmrd 5:72e92c721cd5 670 //angle=left_servo_measured;
labmrd 5:72e92c721cd5 671 int loadcell=adc.read();
labmrd 9:f26a91863e3a 672 //pc.printf("Load Cell Read: %d\n",loadcell);
labmrd 9:f26a91863e3a 673 // if (!calibrateDone){
labmrd 9:f26a91863e3a 674 // calibrateLoadCell();
labmrd 9:f26a91863e3a 675 // }
labmrd 9:f26a91863e3a 676 if (!calibrateDone){
labmrd 9:f26a91863e3a 677 g_command_corrected = 2800;
labmrd 8:cef07a8bdc42 678 // Begin Calibration of load cells
labmrd 8:cef07a8bdc42 679 // Calibrate Load Cells (Does this need to get moved somewhere else?)
labmrd 8:cef07a8bdc42 680 //calibrateLoadCell();
labmrd 8:cef07a8bdc42 681 // These are for load cell initialization of the offset
labmrd 8:cef07a8bdc42 682 //calibrateTotal = (calibrateTotal + loadcell/1000.0)/2.0;
labmrd 9:f26a91863e3a 683
labmrd 9:f26a91863e3a 684 //pc.printf("Load Cell Read: %d\n",loadcell);
labmrd 9:f26a91863e3a 685 loadCellTotal += (long long) loadcell;
labmrd 9:f26a91863e3a 686 calibCntr ++;
labmrd 9:f26a91863e3a 687 if (calibCntr == CALIBRATION_READS){
labmrd 9:f26a91863e3a 688 g_loadCellZero = float(loadCellTotal)/float(CALIBRATION_READS);
labmrd 8:cef07a8bdc42 689 //Update values for thresholding based on calibration
labmrd 9:f26a91863e3a 690 g_thresh_force[0]=g_threshOffset1+g_loadCellZero;
labmrd 9:f26a91863e3a 691 g_thresh_force[1]=g_threshOffset2+g_loadCellZero;
labmrd 9:f26a91863e3a 692 calibrateDone = true;
labmrd 5:72e92c721cd5 693 }
labmrd 5:72e92c721cd5 694 }
labmrd 9:f26a91863e3a 695
labmrd 9:f26a91863e3a 696
labmrd 9:f26a91863e3a 697 #ifdef CALIBRATE_TIME_NOW
labmrd 9:f26a91863e3a 698 if (calibrateDone){
labmrd 9:f26a91863e3a 699 g_command_corrected=potread;
labmrd 9:f26a91863e3a 700 int loadcell_offset = loadcell - g_loadCellZero;
labmrd 9:f26a91863e3a 701 if(loadcell_offset < 0){
labmrd 9:f26a91863e3a 702 loadcell_offset = 0;
labmrd 9:f26a91863e3a 703 }
labmrd 9:f26a91863e3a 704 printf("%6.3f,%0.0f,%2.3f,%2.3f,%d\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell_offset);
labmrd 9:f26a91863e3a 705
labmrd 9:f26a91863e3a 706 }
labmrd 9:f26a91863e3a 707 #else
labmrd 9:f26a91863e3a 708 // Code to determine if we should toggle the variable of "g_we_are_grasping"
labmrd 9:f26a91863e3a 709 if(!g_we_are_grasping && loadcell>g_loadCellZero+g_forceAboveFirstTouch && angledot<velocty_threshold){
labmrd 9:f26a91863e3a 710 //Grasp is starting
labmrd 9:f26a91863e3a 711 g_we_are_grasping=true;
labmrd 9:f26a91863e3a 712 // Set the position and time at first touch
labmrd 9:f26a91863e3a 713 g_masterPositionFirstTouch = potread;
labmrd 9:f26a91863e3a 714 g_timeAtFirstTouch = mytimer.read_us()/1000.0;
labmrd 9:f26a91863e3a 715 for(int i=0;i<NUMBER_OF_TISSUES;i++){
labmrd 9:f26a91863e3a 716 g_error_norm[i]=0.0;
labmrd 9:f26a91863e3a 717 }
labmrd 9:f26a91863e3a 718 }
labmrd 9:f26a91863e3a 719 if(g_we_are_grasping && potread > g_masterPositionFirstTouch && (mytimer.read_us()/1000.0) > (g_timeAtFirstTouch + 200.0)) {//angledot>-velocty_threshold){
labmrd 9:f26a91863e3a 720 //Grasp is over
labmrd 9:f26a91863e3a 721 g_error_norm[0] = 0;
labmrd 9:f26a91863e3a 722 g_error_norm[1] = 0;
labmrd 9:f26a91863e3a 723 g_we_are_grasping=false;
labmrd 9:f26a91863e3a 724 }
labmrd 9:f26a91863e3a 725 float alpha=0;
labmrd 9:f26a91863e3a 726 std::string sstr;
labmrd 9:f26a91863e3a 727 if(g_we_are_grasping){
labmrd 9:f26a91863e3a 728 //D_x = [thetadotdot,thetadot,theta,theta^2,theta^3];
labmrd 9:f26a91863e3a 729 g_error_norm[0]+=fabs(float(loadcell)-(Phi1[0]*angledotdot+Phi1[1]*angledot+Phi1[2]*angle+Phi1[3]*angle*angle+Phi1[4]*angle*angle*angle));
labmrd 9:f26a91863e3a 730 g_error_norm[1]+=fabs(float(loadcell)-(Phi2[0]*angledotdot+Phi2[1]*angledot+Phi2[2]*angle+Phi2[3]*angle*angle+Phi2[4]*angle*angle*angle));
labmrd 9:f26a91863e3a 731
labmrd 9:f26a91863e3a 732
labmrd 9:f26a91863e3a 733 float offset1 = 100000;
labmrd 9:f26a91863e3a 734 float offset2 = 300000;
labmrd 9:f26a91863e3a 735 int tissue_id=0;
labmrd 9:f26a91863e3a 736 if(g_error_norm[1]>g_error_norm[0]){alpha=(g_error_norm[1]-g_error_norm[0])/(g_error_norm[1]+offset1);sstr="HARD";tissue_id=0;}
labmrd 9:f26a91863e3a 737 if(g_error_norm[0]>g_error_norm[1]){alpha=(g_error_norm[0]-g_error_norm[1])/(g_error_norm[0]+offset2);sstr="SOFT";tissue_id=1;}
labmrd 9:f26a91863e3a 738
labmrd 9:f26a91863e3a 739 float force_err=loadcell-g_thresh_force[tissue_id];
labmrd 9:f26a91863e3a 740 float k=20/0.1e6;
labmrd 9:f26a91863e3a 741 g_command_corrected=(1-alpha)*potread+alpha*(k*force_err+angle);
labmrd 9:f26a91863e3a 742 }else{
labmrd 9:f26a91863e3a 743 g_command_corrected=potread;
labmrd 9:f26a91863e3a 744 }
labmrd 5:72e92c721cd5 745
labmrd 9:f26a91863e3a 746 //std::vector<int> data_in=<angle, angledot, angledotdot, loadcell, potread>
labmrd 9:f26a91863e3a 747 int loadcell_offset = loadcell - g_loadCellZero;
labmrd 9:f26a91863e3a 748 if(loadcell_offset < 0){
labmrd 9:f26a91863e3a 749 loadcell_offset = 0;
labmrd 9:f26a91863e3a 750 }
labmrd 9:f26a91863e3a 751 if (!calibrateDone){
labmrd 9:f26a91863e3a 752 g_command_corrected = 2800;
labmrd 9:f26a91863e3a 753 }
labmrd 5:72e92c721cd5 754
labmrd 9:f26a91863e3a 755 printf("Data:,%6.3f,%0.0f,% 2.3f,% 2.3f,%d,%d,%0.0f,%12.3f,%2.3f,%s\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell, potread,g_error_norm[0],g_loadCellZero,alpha,sstr.c_str());
labmrd 9:f26a91863e3a 756 #endif
labmrd 5:72e92c721cd5 757
labmrd 5:72e92c721cd5 758 //printf("%s",recent_pos.get_string().c_str());
labmrd 5:72e92c721cd5 759 // printf("<%d,%d,%d,%d,%d,%d,%d> ",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos,
labmrd 5:72e92c721cd5 760 // tempSpindleData.myServoData[LEFT_SERVO_INDEX].force,
labmrd 5:72e92c721cd5 761 // tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos,
labmrd 5:72e92c721cd5 762 // tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force,
labmrd 5:72e92c721cd5 763 // tempSpindleData.time,
labmrd 5:72e92c721cd5 764 // tempSpindleData.direction,
labmrd 5:72e92c721cd5 765 // tempSpindleData.cycle);
labmrd 9:f26a91863e3a 766 // printf(" %dus", worst_latency);
labmrd 5:72e92c721cd5 767 // worst_latency=0;
labmrd 5:72e92c721cd5 768 // printf(" %0.0fbps\n", bits_received*1000.0/AuxSerialTimer.read_ms());
labmrd 5:72e92c721cd5 769 // bits_received=0;
labmrd 2:dfeadd6c651c 770 AuxSerialTimer.reset();
labmrd 2:dfeadd6c651c 771 }
labmrd 2:dfeadd6c651c 772
mbed_official 0:8a555873b7d3 773 }
mbed_official 0:8a555873b7d3 774 }