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@12:560dc69d4ca4, 2015-07-20 (annotated)

Committer:
labmrd
Date:
Mon Jul 20 17:12:36 2015 +0000
Revision:
12:560dc69d4ca4
Parent:
11:53408545bbb4
Child:
13:6a0a7a04fd91
Changed the collaborative control scheme to include integrator. It is now a PI scheme. First attempt at this PI scheme. Previous versions will only be proportional controller.

Who changed what in which revision?

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