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@7:b4b95e5f0fce, 2015-05-06 (annotated)

Committer:
labmrd
Date:
Wed May 06 20:21:32 2015 +0000
Revision:
7:b4b95e5f0fce
Parent:
6:b4347c69816a
Child:
8:cef07a8bdc42
Added the load cell calibration function....BUT......don't run it. I need to fix the load cells from colliding into each as soon as it calls the calibration function.

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