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@8:cef07a8bdc42, 2015-05-11 (annotated)

Committer:
labmrd
Date:
Mon May 11 21:39:45 2015 +0000
Revision:
8:cef07a8bdc42
Parent:
7:b4b95e5f0fce
Child:
9:f26a91863e3a
The criteria for grasping was changed to trigger off of the master as opposed to the slave.  The calibration runs but seems to read high. FIX THIS!

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