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

Committer:
labmrd
Date:
Tue May 05 18:53:06 2015 +0000
Revision:
6:b4347c69816a
Parent:
5:72e92c721cd5
Child:
7:b4b95e5f0fce
Changed serial output to include "Data:," at the beginning in order for easier parsing for visual display in Processing. Nothing is broken.

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