MRD Lab
A code for the spindling of bots.
Dependencies: MX12 ServoRingBuffer mbed-src
Fork of
SpindleBot
by 
MRD Lab
Revision 14:7c5beaa9fb01, committed 2015-08-13
- Comitter:
- labmrd
- Date:
- Thu Aug 13 17:55:40 2015 +0000
- Parent:
- 13:6a0a7a04fd91
- Commit message:
- This revision marks Mark's mark of resignation from the labmrd mbed account.
Changed in this revision
diff -r 6a0a7a04fd91 -r 7c5beaa9fb01 MX12.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MX12.lib Thu Aug 13 17:55:40 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/labmrd/code/MX12/#4c118a827f11
diff -r 6a0a7a04fd91 -r 7c5beaa9fb01 ServoRingBuffer.lib --- a/ServoRingBuffer.lib Thu Aug 13 17:26:23 2015 +0000 +++ b/ServoRingBuffer.lib Thu Aug 13 17:55:40 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/labmrd/code/ServoRingBuffer/#da00ed8a1cd5 +http://developer.mbed.org/users/labmrd/code/ServoRingBuffer/#f109687a28fe
diff -r 6a0a7a04fd91 -r 7c5beaa9fb01 data_set/data_set.cpp
--- a/data_set/data_set.cpp Thu Aug 13 17:26:23 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,298 +0,0 @@
-#include "data_set.h"
-#include "math.h"
-
-data_set::data_set()
-{
- data_size=10;
- current_position=0;
- data_x.resize(data_size);
- data_y.resize(data_size);
- for(int i=0;i<data_size;i++){
- data_x.at(i)=0.0;
- data_y.at(i)=0.0;
- }
-}
-
-data_set::data_set(int size,std::string new_title,std::string new_ylabel,std::string new_xlabel)
-{
- data_size=size;
- current_position=0;
- data_x.resize(data_size);
- data_y.resize(data_size);
- for(int i=0;i<data_size;i++){
- data_x.at(i)=0.0;
- data_y.at(i)=0.0;
- }
- title=new_title;
- xlabel=new_xlabel;
- ylabel=new_ylabel;
-}
-
-void data_set::add_data(float value_y,float value_x)
-{
- if(++current_position>=data_size)
- current_position=0;
- data_x.at(current_position)=value_x;
- data_y.at(current_position)=value_y;
-}
-
-float data_set::min(void){
- float minimum=data_y.at(0);
- for(int i=0;i<data_size;i++)
- if(data_y.at(i)<minimum)
- minimum=data_y.at(i);
- return minimum;
-}
-
-float data_set::max(void){
- float maximum=data_y.at(0);
- for(int i=0;i<data_size;i++)
- if(data_y.at(i)>maximum)
- maximum=data_y.at(i);
- return maximum;
-}
-
-float data_set::xmin(void){
- float minimum=data_x.at(0);
- for(int i=0;i<data_size;i++)
- if(data_x.at(i)<minimum)
- minimum=data_x.at(i);
- return minimum;
-}
-
-float data_set::xmax(void){
- float maximum=data_x.at(0);
- for(int i=0;i<data_size;i++)
- if(data_x.at(i)>maximum)
- maximum=data_x.at(i);
- return maximum;
-}
-
-float data_set::mean(void)
-{
- float mean=0.0;
- for(int i=0;i<data_size;i++)
- mean+=data_y.at(i);
- mean/=data_size;
- return mean;
-}
-
-float data_set::stdev(void)
-{
- float stdev=0.0;
- float avg=mean();
- for(int i=0;i<data_size;i++)
- stdev+=(data_y.at(i)-avg)*(data_y.at(i)-avg);
- stdev/=data_size;
- stdev=sqrt(stdev);
- return stdev;
-}
-
-void data_set::least_squares(float &a0,float &a1){
- float sumx=0.0;
- float sumy=0.0;
- float sumxy=0.0;
- float sumx2=0.0;
- for(int i=0;i<data_size;i++)
- {
- sumx+=data_x.at(i);
- sumy+=data_y.at(i);
- sumxy+=data_x.at(i)*data_y.at(i);
- sumx2+=data_x.at(i)*data_x.at(i);
- }
- a0=(sumy*sumx2-sumx*sumxy)/(data_size*sumx2-sumx*sumx);
- a1=(data_size*sumxy-sumx*sumy)/(data_size*sumx2-sumx*sumx);
-}
-
-data_set data_set::get_fit(void){
- float m,b;
- least_squares(b,m);
- data_set fit(data_size,"Linear Fit");
- for(int i=0;i<data_size;i++)
- fit.add_data(m*data_x.at(i)+b,data_x.at(i));
- return fit;
-}
-
-std::string data_set::get_string(void){
- std::stringstream ss;
- for(int i=0;i<data_size;i++)
- {
- ss<<i<<","<<data_x.at(i)<<","<<data_y.at(i)<<std::endl;
- }
- return ss.str();
-}
-
-
-bool data_set::least_squares_3rd(float coeffs[4])
-{
- // I honestly don't know if we are using row major or
- // column major. But it works?
- float xt_x[16],inv[16],xt_y[4];
-
- // Calculate X'*X
- for(int ii=0;ii<4;ii++){
- for(int jj=0;jj<4;jj++){
- xt_x[ii*4+jj]=0;
- for(int i=0;i<data_size;i++){
- // could replace with x1=pow(data_x.at(i),ii);
- float x1=1;for(int p=0;p<ii;p++){x1*=data_x.at(i);}
- float x2=1;for(int p=0;p<jj;p++){x2*=data_x.at(i);}
- xt_x[ii*4+jj]+=x1*x2;
- }
- }
- }
-
- // Invert (X'*X)
- if(!Invert4x4Matrix(xt_x,inv)){
- return false;
- }
-
- // Calculate X'*y
- for(int ii=0;ii<4;ii++){
- xt_y[ii]=0;
- for(int i=0;i<data_size;i++){
- float x1=1;for(int p=0;p<ii;p++){x1*=data_x.at(i);}
- xt_y[ii]+=x1*data_y.at(i);
- }
- }
-
- // Multiply final product: [inv(X'*X)] * [X'*y]
- for(int ii=0;ii<4;ii++){
- coeffs[ii]=0;
- for(int jj=0;jj<4;jj++){
- coeffs[ii]+=inv[ii*4+jj]*xt_y[jj];
- }
- }
-
- return true;
-}
-
-bool data_set::Invert4x4Matrix(const float m[16], float invOut[16])
-{
- float inv[16], det;
- int i;
-
- inv[0] = m[5] * m[10] * m[15] -
- m[5] * m[11] * m[14] -
- m[9] * m[6] * m[15] +
- m[9] * m[7] * m[14] +
- m[13] * m[6] * m[11] -
- m[13] * m[7] * m[10];
-
- inv[4] = -m[4] * m[10] * m[15] +
- m[4] * m[11] * m[14] +
- m[8] * m[6] * m[15] -
- m[8] * m[7] * m[14] -
- m[12] * m[6] * m[11] +
- m[12] * m[7] * m[10];
-
- inv[8] = m[4] * m[9] * m[15] -
- m[4] * m[11] * m[13] -
- m[8] * m[5] * m[15] +
- m[8] * m[7] * m[13] +
- m[12] * m[5] * m[11] -
- m[12] * m[7] * m[9];
-
- inv[12] = -m[4] * m[9] * m[14] +
- m[4] * m[10] * m[13] +
- m[8] * m[5] * m[14] -
- m[8] * m[6] * m[13] -
- m[12] * m[5] * m[10] +
- m[12] * m[6] * m[9];
-
- inv[1] = -m[1] * m[10] * m[15] +
- m[1] * m[11] * m[14] +
- m[9] * m[2] * m[15] -
- m[9] * m[3] * m[14] -
- m[13] * m[2] * m[11] +
- m[13] * m[3] * m[10];
-
- inv[5] = m[0] * m[10] * m[15] -
- m[0] * m[11] * m[14] -
- m[8] * m[2] * m[15] +
- m[8] * m[3] * m[14] +
- m[12] * m[2] * m[11] -
- m[12] * m[3] * m[10];
-
- inv[9] = -m[0] * m[9] * m[15] +
- m[0] * m[11] * m[13] +
- m[8] * m[1] * m[15] -
- m[8] * m[3] * m[13] -
- m[12] * m[1] * m[11] +
- m[12] * m[3] * m[9];
-
- inv[13] = m[0] * m[9] * m[14] -
- m[0] * m[10] * m[13] -
- m[8] * m[1] * m[14] +
- m[8] * m[2] * m[13] +
- m[12] * m[1] * m[10] -
- m[12] * m[2] * m[9];
-
- inv[2] = m[1] * m[6] * m[15] -
- m[1] * m[7] * m[14] -
- m[5] * m[2] * m[15] +
- m[5] * m[3] * m[14] +
- m[13] * m[2] * m[7] -
- m[13] * m[3] * m[6];
-
- inv[6] = -m[0] * m[6] * m[15] +
- m[0] * m[7] * m[14] +
- m[4] * m[2] * m[15] -
- m[4] * m[3] * m[14] -
- m[12] * m[2] * m[7] +
- m[12] * m[3] * m[6];
-
- inv[10] = m[0] * m[5] * m[15] -
- m[0] * m[7] * m[13] -
- m[4] * m[1] * m[15] +
- m[4] * m[3] * m[13] +
- m[12] * m[1] * m[7] -
- m[12] * m[3] * m[5];
-
- inv[14] = -m[0] * m[5] * m[14] +
- m[0] * m[6] * m[13] +
- m[4] * m[1] * m[14] -
- m[4] * m[2] * m[13] -
- m[12] * m[1] * m[6] +
- m[12] * m[2] * m[5];
-
- inv[3] = -m[1] * m[6] * m[11] +
- m[1] * m[7] * m[10] +
- m[5] * m[2] * m[11] -
- m[5] * m[3] * m[10] -
- m[9] * m[2] * m[7] +
- m[9] * m[3] * m[6];
-
- inv[7] = m[0] * m[6] * m[11] -
- m[0] * m[7] * m[10] -
- m[4] * m[2] * m[11] +
- m[4] * m[3] * m[10] +
- m[8] * m[2] * m[7] -
- m[8] * m[3] * m[6];
-
- inv[11] = -m[0] * m[5] * m[11] +
- m[0] * m[7] * m[9] +
- m[4] * m[1] * m[11] -
- m[4] * m[3] * m[9] -
- m[8] * m[1] * m[7] +
- m[8] * m[3] * m[5];
-
- inv[15] = m[0] * m[5] * m[10] -
- m[0] * m[6] * m[9] -
- m[4] * m[1] * m[10] +
- m[4] * m[2] * m[9] +
- m[8] * m[1] * m[6] -
- m[8] * m[2] * m[5];
-
- det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
-
- if (det == 0)
- return false;
-
- det = 1.0 / det;
-
- for (i = 0; i < 16; i++)
- invOut[i] = inv[i] * det;
-
- return true;
-}
diff -r 6a0a7a04fd91 -r 7c5beaa9fb01 data_set/data_set.h
--- a/data_set/data_set.h Thu Aug 13 17:26:23 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-#ifndef DATA_SET_H
-#define DATA_SET_H
-
-#include <vector>
-#include <sstream>
-
-class data_set
-{
-public:
- data_set();
- data_set(int size,std::string new_title="",std::string new_ylabel="y",std::string new_xlabel="x");
- void add_data(float value_y,float value_x=0.0);
- float min(void);
- float max(void);
- float xmin(void);
- float xmax(void);
- float mean(void);
- float stdev(void);
- void least_squares(float &a0,float &a1);
- data_set get_fit(void);
- std::string get_string(void);
- bool least_squares_3rd(float coeffs[4]);
- bool Invert4x4Matrix(const float m[16], float invOut[16]);
-
- int data_size;
- int current_position;
- std::vector<float> data_x;
- std::vector<float> data_y;
- std::string title;
- std::string xlabel;
- std::string ylabel;
-};
-
-#endif // DATA_SET_H
diff -r 6a0a7a04fd91 -r 7c5beaa9fb01 main.cpp
--- a/main.cpp Thu Aug 13 17:26:23 2015 +0000
+++ b/main.cpp Thu Aug 13 17:55:40 2015 +0000
@@ -1,9 +1,6 @@
-// Different modes to select
-#define USE_DYNAMIXELS
-//#define USE_BLUETOOTH
-//#define USE_SD_CARD
-#define ROD_IS_RIGHT
-//#define CALIBRATE_TIME_NOW
+//#define USE_DYNAMIXELS
+#define USE_BLUETOOTH
+#define USE_SD_CARD
// We have different modes for different things
#define MODE_MANUAL 1
@@ -22,11 +19,11 @@
#include "ServoRingBuffer.h"
#include "ram_test.h"
#include "Serial_Receive.h"
-#include "data_set.h"
#include <string>
// Specific to Dynamixels
#ifdef USE_DYNAMIXELS
+#include "MX12.h"
#include "AD7730.h"
#endif
@@ -51,24 +48,22 @@
STATE_OPEN_HOLD=3
};
+
// Define pins and interrupts
-Ticker potISR; //Define a recurring timer interrupt
-DigitalOut led1(LED1); //Led 1 for debugging purposes
-DigitalOut led2(LED2); //Led 2 for debugging purposes
-DigitalOut led3(LED3); //Led 3 for debugging purposes
-DigitalOut led4(LED4); //Led 4 for debugging purposes
+Ticker potISR; //Define a recurring timer interrupt
+DigitalOut led1(LED1); //Led 1 for debugging purposes
+DigitalOut led2(LED2); //Led 2 for debugging purposes
+DigitalOut led3(LED3); //Led 3 for debugging purposes
+//DigitalOut led4(LED4); //Led 4 for debugging purposes
DigitalOut triggerOut(p11);
-Serial pc(USBTX, USBRX); //Set up serial connection to pc
-AnalogIn AinLeftForce(p16); //Set up left load cell on pin 16
-AnalogIn AinRightForce(p15); //Set up right load cell on pin 15
-DigitalOut failSafePowerSwitch(p29); //Set up pin 28 to send HIGH to OpenCM when MBED has power
-
-// Specific to bluetooth
+Serial pc(USBTX, USBRX); //Set up serial connection to pc
#ifdef USE_BLUETOOTH
Serial bt(p13,p14); //Set up serial connection to bluetooth adapter
#endif
-// Specific to SD Card
+AnalogIn AinLeftForce(p16); //Set up potentiometer on pin 20
+AnalogIn AinRightForce(p15); //Set up potentiometer on pin 20
+
#ifdef USE_SD_CARD
// Attach SD card
SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
@@ -81,6 +76,13 @@
float max_percent_full=0;
// Define variables for the program
+float servoAngle; //This is the desired servo angle based on the scaled potentiometer value
+float potData; //This is the value of the potentiometer from Ain.read()
+bool collect_data = false; //This is
+
+bool keyStrokeFlag = false; //This is a flag to see if a keystroke has been pressed
+char keyStrokeVal; //This is a character storing the value of the keystroke
+
char g_tissue_type_name[32];
float g_frequency;
int g_max_force;
@@ -91,130 +93,36 @@
unsigned char g_current_mode=MODE_NULL;
jaw_state g_current_direction=STATE_OPEN_HOLD;
unsigned char g_current_cycle=0;
-int g_input_pot_1;
-#define NUMBER_OF_TISSUES 2
-float g_error_norm[NUMBER_OF_TISSUES];
-bool g_we_are_grasping=false;
-#define CALIBRATION_READS 200.0
-float g_calibration_offset = 8600000.0;
-float integral_err = 0.0;
-float g_loadCellSlopeCalibration = 2.1e-5;
-float forceInNewtons = 0.0;
-float g_gravity = 9.81;
-
-// Values at first touch
-int g_masterPositionFirstTouch = 2400;
-int g_slavePositionFirstTouch = 2400;
-float g_forceAboveFirstTouch = 3.5;
-float g_timeAtFirstTouch;
-
-///////////Magic numbers courtesy Rod///////////////
-//float Phi1[5]={-8.02086003501975e-08,
-// 1.55286905458007e-05,
-// 0.00795344249784777,
-// 8.23733045077119,
-// -0.00299236282304311};
-//float Phi2[5]={-1.24436831310503e-08,
-// 1.23673348605010e-05,
-// 0.00652545188345528,
-// 6.75893262890734,
-// -0.00228098997419065};
-
-/////////// Zero-Offset Magic Numbers courtesy Rod//////////////
-float Phi1[5] = {-1.80212053214826e-08,
- 1.69579390650964e-06,
- 0.000592679062823746,
- 0.624774980172511,
- -0.000283294192960159};
-
-float Phi2[5] = {1.00123534796440e-09,
- 1.42089516619424e-06,
- 0.000520811899959219,
- 0.542284752693981,
- -0.000248770560431049};
-
-// Magic numbers to determine if we are grasping (CHANGE TO A DYNAMIC APPROACH!!!!)
-//float entry_threshold=8.70e6;
-float velocity_threshold=-0.01;
-
-float g_thresh_force[NUMBER_OF_TISSUES]={0.16e6,0.06e6};
-///////////Magic numbers courtesy Rod///////////////
-
-int g_command_corrected;
-
-// These are for load cell initialization of the offset
-float g_loadCellZero = 8600000.0;
-bool calibrateDone = false;
-//long calibrateTotal = 8600;
-int calibCntr = 0;
-//int g_loadCellOffset = 100000;
-//int g_threshOffset1 = 0.2e6;
-//int g_threshOffset2 = 0.15e6;
-long long loadCellTotal = 0;
-
-Timer mytimer;
// Warning, this buffer is large!
ServoRingBuffer Buffer;
spindleData tempSpindleData; //For sending to the buffer
-#ifdef ROD_IS_RIGHT
- int angle_sum=0;
- int angle_count=0;
- int angledot_sum=0;
- int angledot_count=0;
- int last_angle=0;
- int angledotdot_sum=0;
- int angledotdot_count=0;
- int last_angledot=0;
-#else
- data_set recent_pos(30,"Data","Time (us)","Position (encoder counts)");
- float coeffs[4];
- bool crunching_data_flag;
-#endif
-
-int bits_received;
Timer ISRDurationTimer;
Timer AuxSerialTimer;
int worst_latency=0;
int current_latency;
-// Specific to Dynamixels
#ifdef USE_DYNAMIXELS
- Serial cm(p28,p27); //Set up serial connection to OpenCM 9.04
- unsigned short left_servo_measured=0;
- unsigned short right_servo_measured=0;
- char input_buffer[16];
- int input_buffer_location=0;
-
- float samplingPeriod = 0.001; //This is the sampling period for the timer interrupt
+ //Dynamixels can only handle 500Hz for now. Working on it...
+ float samplingPeriod = 0.005; //This is the sampling period for the timer interrupt
#define LEFT_JAW_DYNAMIXEL_ID 3
#define RIGHT_JAW_DYNAMIXEL_ID 4
-// #define CLOSED_SERVO_ANGLE_LEFT 1001 //This is the closed in encoder counts
-// #define OPEN_SERVO_ANGLE_LEFT 2663 //This is the open in encoder counts
-// #define CLOSED_SERVO_ANGLE_RIGHT 3259 //This is the closed in encoder counts
-// #define OPEN_SERVO_ANGLE_RIGHT 1486 //This is the open in encoder counts
- #define CLOSED_SERVO_ANGLE_LEFT 1975 //This is the closed in encoder counts
- #define OPEN_SERVO_ANGLE_LEFT 2560 //This is the open in encoder counts
- #define CLOSED_SERVO_ANGLE_RIGHT 1975 //This is the closed in encoder counts
- #define OPEN_SERVO_ANGLE_RIGHT 2560 //This is the open in encoder counts
- //AD7730( mosi, miso, sclk, ready, cs)
- AD7730 adc(p11, p12, p13, p14, p15);
- //AD7730 adc2(p11, p12, p13, p18, p19);
+ #define CLOSED_SERVO_ANGLE_LEFT 1121 //This is the closed in encoder counts
+ #define OPEN_SERVO_ANGLE_LEFT 2783 //This is the open in encoder counts
+ #define CLOSED_SERVO_ANGLE_RIGHT 3259 //This is the closed in encoder counts
+ #define OPEN_SERVO_ANGLE_RIGHT 1486 //This is the open in encoder counts
+ // Dynamixel Object
+ MX12 mx12_left_jaw (p28, p27, p30, p29, LEFT_JAW_DYNAMIXEL_ID, 1000000);
+ MX12 mx12_right_jaw (p28, p27, p30, p29, RIGHT_JAW_DYNAMIXEL_ID, 1000000);
-
- AnalogIn AinJoystickFwdBk(p17); //Set up potentiometer on pin 17
- AnalogIn AinJoystickLftRt(p16); //Set up potentiometer on pin 16
- float JoystickFwdBk_Zero=0.5;
- float JoystickLftRt_Zero=0.5;
+ AD7730 adc(p9, p26, p11, p12, p25);
/// Set these to inputs so that they don't interfere with the serial communication
DigitalIn nullOut1(p21);
DigitalIn nullOut2(p22);
DigitalIn nullOut3(p23);
DigitalIn nullOut4(p24);
- /// This one is in the way of the SMD pads
- DigitalIn nullOut5(p20);
#else
float samplingPeriod = 0.001; //This is the sampling period for the timer interrupt
#define SERVO_DEGREE_0 900 //This is the pulse width value for HiTEC-422 in microseconds to turn 0 degrees
@@ -243,42 +151,6 @@
#endif
-// Serial interrupt function
-void serialInterrupt(char buffer){
- input_buffer[input_buffer_location]=buffer;
- input_buffer_location++;
- bits_received+=8;
- //pc.printf("RC:%d\n",buffer);
-
-
- //Is the packet looking good so far??
- if(input_buffer[0]==0xFF){
- //Is the packet looking good so far??????
- if(input_buffer[1]==0xFF || input_buffer_location ==1){
- //Do we have a complete packet??
- if(input_buffer_location>=6){ //This is 6 because we already incremented
- //We do! Extract the juicy datas
- left_servo_measured = ( input_buffer[2] << 8 ) | input_buffer[3];
- right_servo_measured = ( input_buffer[4] << 8 ) | input_buffer[5];
- //pc.printf("RP:%d,%d\n",left_servo_measured,right_servo_measured);
- //Reset the buffer location so we can start over
- input_buffer_location=0;
- }
- }else{
- //Something is wrong. We may just not be at the correct location in the packet
- //Reset the buffer location so we can start over
- input_buffer_location=0;
- //printf("Error, Byte 2 not what was expected: 0xFF!=0x%02x\n",input_buffer[1]);
- }
- }else{
- //Something is wrong. We may just not be at the correct location in the packet
- //Reset the buffer location so we can start over
- input_buffer_location=0;
- //printf("Error, Byte 1 not what was expected: 0xFF!=0x%02x\n",input_buffer[0]);
- }
-}
-
-
// Function trapezoidalTrajectory: Function that takes in a time (float in seconds) and outputs a float (0 to 1) that corresponds to a trapezoidal trajectory
float trapezoidalTrajectory(float t, jaw_state &state, unsigned char &cycle_num) {
// Define variables specific to this function
@@ -291,7 +163,7 @@
// Take the time and mod it with the period to be able to break up each cycle into 4 piecewise sections
timeMod = fmodf(t,period);
- // Determining trajectory
+ //
if (timeMod < period/4.0)
{
y_trapezoid = (-4.0/period)*(timeMod)+1.0;
@@ -323,7 +195,7 @@
// Function timerISRFunction: Timer ISR function to collect data and write to ring buffer
void timerISRFunction() {
- //if (calibrateDone){
+ if(collect_data){
//led 1 is used as a 'thinking' light, brighter=worse
led1 = 1;
led2 = 0;
@@ -331,143 +203,50 @@
ISRDurationTimer.reset();
ISRDurationTimer.start();
- if(g_current_mode==MODE_AUTOMATIC){
-
- // Warning, this calculation is in the ISR and as such is probably slower than we would prefer.
- // @todo The math could certainly be optimized with some precalculated constants. Lookup tables are faster than sin()
- float percent=trapezoidalTrajectory(g_current_trajectory_time,g_current_direction,g_current_cycle);
- g_current_trajectory_time+=samplingPeriod;
-
-
- //float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
- //g_current_direction=(cos(angle)<0);
- //g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
-
-
- #ifdef USE_DYNAMIXELS
- //float percent=(sin(angle)+1)/2.0;
- if(adc.isReady()){
- adc.interruptRead();
- }
-
- while(cm.readable()){
- led4=1;
- serialInterrupt(cm.getc());
- }
- led4=0;
-
- short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
- short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
-
- //Send the commanded position to the OpenCM board
- cm.putc(0xFF);
- cm.putc(0xFF);
- cm.putc(left_servo >> 8); //Top 4 bits
- cm.putc(left_servo & 0xFF); //Bottom 8 bits
- cm.putc(right_servo >> 8); //Top 4 bits
- cm.putc(right_servo & 0xFF); //Bottom 8 bits
-
- tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = adc.read();
- tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = left_servo_measured;
- tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = 0;
- tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = right_servo_measured;
- tempSpindleData.direction=g_current_direction;
- tempSpindleData.cycle=g_current_cycle;
- Buffer.write(tempSpindleData);
- #else
- g_theta=(1.0-percent)*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI;
- tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = AinLeftForce.read_u16();
- tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = AinLeftPosition.read_u16();
- tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
- tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = AinRightPosition.read_u16();
- tempSpindleData.direction=g_current_direction;
- tempSpindleData.cycle=g_current_cycle;
- Buffer.write(tempSpindleData);
-
-
-
- moveServoTo(g_theta); // in degrees, son
- #endif
- }else if(g_current_mode==MODE_MANUAL){
-
- g_current_trajectory_time+=samplingPeriod;
+
+ // Warning, this calculation is in the ISR and as such is probably slower than we would prefer.
+ // @todo The math could certainly be optimized with some precalculated constants. Lookup tables are faster than sin()
+ float percent=trapezoidalTrajectory(g_current_trajectory_time,g_current_direction,g_current_cycle);
+ g_current_trajectory_time+=samplingPeriod;
+
+
+ //float angle=g_current_trajectory_time*g_frequency*2.0*M_PI-M_PI_2;
+ //g_current_direction=(cos(angle)<0);
+ //g_current_cycle=(angle+M_PI_2)/(2.0*M_PI);
+
+
+ #ifdef USE_DYNAMIXELS
+ //float percent=(sin(angle)+1)/2.0;
if(adc.isReady()){
adc.interruptRead();
}
- int im_tired_of_this_game=0;
- while(cm.readable() && im_tired_of_this_game++<100){
- serialInterrupt(cm.getc());
- }
-
- // float pot_open=0.75;
- // float pot_closed=0.48;
- //
- // float percent=AinJoystickFwdBk.read();
- // percent=(pot_open-percent)/(pot_open-pot_closed);
- // float skew =0;//AinJoystickLftRt.read()-JoystickLftRt_Zero;
- //
- // // The 'pulling' of the trigger corresponds to open/closed
- // short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
- // short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
- //
- // g_input_pot_1=left_servo;
- // Set limits on the max and min value of the grasper (for safety reasons)
- if (g_command_corrected < 1900){g_command_corrected = 1900;};
- if (g_command_corrected > 2900){g_command_corrected = 2900;};
- short left_servo =g_command_corrected;
- short right_servo=g_command_corrected;
+ short left_servo =percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
+ short right_servo=percent*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT)+OPEN_SERVO_ANGLE_RIGHT;
+ mx12_right_jaw.coordinated_move(LEFT_JAW_DYNAMIXEL_ID,left_servo, 0, RIGHT_JAW_DYNAMIXEL_ID,right_servo, 0);
- // // The 'skewing' of the trigger corresponds to yawing the jaws left or right, while maintaining open/closed
- // left_servo +=skew*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT );
- // right_servo-=skew*(CLOSED_SERVO_ANGLE_RIGHT-OPEN_SERVO_ANGLE_RIGHT);
- //
- // /// Todo: There is undoubtedly a cleaner way to do this.
- // if(OPEN_SERVO_ANGLE_LEFT < CLOSED_SERVO_ANGLE_LEFT && left_servo < OPEN_SERVO_ANGLE_LEFT ){left_servo =OPEN_SERVO_ANGLE_LEFT; }
- // if(OPEN_SERVO_ANGLE_LEFT > CLOSED_SERVO_ANGLE_LEFT && left_servo > OPEN_SERVO_ANGLE_LEFT ){left_servo =OPEN_SERVO_ANGLE_LEFT; }
- // if(OPEN_SERVO_ANGLE_RIGHT < CLOSED_SERVO_ANGLE_RIGHT && right_servo < OPEN_SERVO_ANGLE_RIGHT){right_servo=OPEN_SERVO_ANGLE_RIGHT;}
- // if(OPEN_SERVO_ANGLE_RIGHT > CLOSED_SERVO_ANGLE_RIGHT && right_servo > OPEN_SERVO_ANGLE_RIGHT){right_servo=OPEN_SERVO_ANGLE_RIGHT;}
- //
- // // Just to make sure
- // // left_servo=(left_servo+4096)%4096;
- // if(left_servo < 0){left_servo = 0;}
- // if(left_servo >4095){left_servo =4095;}
- // if(right_servo< 0){right_servo= 0;}
- // if(right_servo>4095){right_servo=4095;}
-
- //Send the commanded position to the OpenCM board
- cm.putc(0xFF);
- cm.putc(0xFF);
- cm.putc(left_servo >> 8); //Top 4 bits
- cm.putc(left_servo & 0xFF); //Bottom 8 bits
- cm.putc(right_servo >> 8); //Top 4 bits
- cm.putc(right_servo & 0xFF); //Bottom 8 bits
+// tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = adc.read();
+// tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = mx12_left_jaw.GetRawPosition();
+// tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
+// tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = mx12_right_jaw.GetRawPosition();
+// tempSpindleData.direction=g_current_direction;
+// tempSpindleData.cycle=g_current_cycle;
+// Buffer.write(tempSpindleData);
+ #else
+ g_theta=(1.0-percent)*(MAX_SERVO_ANGLE_Da_VINCI-MIN_SERVO_ANGLE_Da_VINCI)+MIN_SERVO_ANGLE_Da_VINCI;
+ tempSpindleData.myServoData[LEFT_SERVO_INDEX].force = AinLeftForce.read_u16();
+ tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos = AinLeftPosition.read_u16();
+ tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force = AinRightForce.read_u16();
+ tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos = AinRightPosition.read_u16();
+ tempSpindleData.direction=g_current_direction;
+ tempSpindleData.cycle=g_current_cycle;
+ Buffer.write(tempSpindleData);
- #ifdef ROD_IS_RIGHT
- //Famous Rod's Magical World of Good Ideas Part 1
- int angle=left_servo_measured;
- angle_sum+=angle;
- if(angle_count!=0){
- int angledot=(angle-last_angle);
- angledot_sum+=angledot;
- if(angledot_count!=0){
- int angledotdot=(angledot-last_angledot);
- angledotdot_sum+=angledotdot;
- angledotdot_count++;
- }
- angledot_count++;
- last_angledot=angledot;
- }
- angle_count++;
- last_angle=angle;
- #else
- // John's worse than Rod ideas of terribleness
- if(!crunching_data_flag){ // Only add data if no one is using it
- recent_pos.add_data(left_servo_measured,g_current_trajectory_time);
- }
- #endif
- }
+
+ moveServoTo(g_theta); // in degrees, son
+ #endif
+
//done thinking
led1 = 0;
led2 = 1;
@@ -478,43 +257,20 @@
if(current_latency>worst_latency){
worst_latency=current_latency;
}
- //}
+ }
}
-// Calibrate Load Cell Function (NOT USING THIS ONE!!!)
-void calibrateLoadCell() {
- long long loadCellTotal = 0;
- int intermediateValue;
- for(int ii=0;ii<CALIBRATION_READS;ii++){
- intermediateValue = adc.read();
- //pc.printf("Load Cell Read: %i\n",intermediateValue);
- loadCellTotal+= intermediateValue;
- wait_ms(2);
- //pc.printf("We are calibrating load cells...\n");
- }
- g_calibration_offset=float(loadCellTotal)/float(CALIBRATION_READS);
- //Update values for thresholding based on calibration
- //g_thresh_force[0]=g_threshOffset1+g_calibration_offset;
- //g_thresh_force[1]=g_threshOffset2+g_calibration_offset;
- calibrateDone = true;
-}
int main() {
// Crazy fast baud rate!
- pc.baud(115200*8);
-
- // For communication with OpenCM 9.04 Board
- cm.baud(1000000);
+ pc.baud(921600);
#ifdef USE_BLUETOOTH
bt.baud(9600);
#endif
// Attach ISR routines
- potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod
-
- // Turn p28 to HIGH signaling the OpenCM to take commands from MBED
- failSafePowerSwitch = 1;
+ potISR.attach(&timerISRFunction, samplingPeriod); // setup serialPot to call every samplingPeriod
// Some debug info:
//DisplayRAMBanks();
@@ -543,39 +299,44 @@
pc.printf("\n\n");
#ifdef USE_DYNAMIXELS
+ mx12_left_jaw.Init();
+ //mx12_left_jaw.SetBaud(3000000);
+ //mx12_left_jaw.SetBaud(1000000);
+ //printf("Current Position=%1.3f\n",mx12_left_jaw.GetPosition());
+ mx12_right_jaw.Set_Return_Delay_Time(0.050);
+ printf("Current ReturnDelay=%f ms\n",mx12_left_jaw.Get_Return_Delay_Time());
+ mx12_left_jaw.Set_Return_Delay_Time(0.050);
+ //mx12_left_jaw.Set_Torque_Limit(99.9);
+ //mx12_right_jaw.Set_Torque_Limit(99.9);
+ mx12_left_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
+ mx12_right_jaw.write_short(MX12_REG_MAX_TORQUE,0x03FF);
+ mx12_left_jaw.Set_P_Gain(4);
+ mx12_right_jaw.Set_P_Gain(4);
+ mx12_left_jaw.Set_I_Gain(8);
+ mx12_right_jaw.Set_I_Gain(8);
+ mx12_left_jaw.Set_Alarm_Shutdown(0x04);
+ mx12_right_jaw.Set_Alarm_Shutdown(0x04);
+
+ mx12_left_jaw.Dump_OD_to_Serial(pc);
+ mx12_right_jaw.Dump_OD_to_Serial(pc);
+
+
+
adc.setFilter(256 , false, 1);
adc.start();
-
- JoystickLftRt_Zero=0;
- int calib_N=100;
- for(int ii=0;ii<calib_N;ii++){JoystickLftRt_Zero+=AinJoystickLftRt.read();}
- JoystickLftRt_Zero=JoystickLftRt_Zero/calib_N;
- g_current_mode=MODE_MANUAL;
#else
// Configure Servo for HiTec 422
myServoLeft.period_ms(20);
myServoRight.period_ms(20);
#endif
-
-
printf("Setup Complete.\n");
AuxSerialTimer.start();
- mytimer.start();
-
- //wait_ms(1000);
- //calibrateLoadCell(); // This function did not work as well as having it in the main loop
while(1)
{
// spin in a main loop. serialISR will interrupt it to call serialPot
-
-
- #ifdef USE_DYNAMIXELS
-
- #endif
-
///This checks for any new serial bytes, and returns true if
///we have an entire packet ready. The new packet will live
///in newData.
@@ -596,6 +357,7 @@
g_num_cycles=newData[3];
g_current_trajectory_time=0;
g_current_cycle=0;
+ g_current_mode=MODE_AUTOMATIC;
#ifdef USE_SD_CARD
int first_slash=file_name_in.find("/");
std::string new_dir="/sd/"+file_name_in.substr(0, first_slash);
@@ -620,13 +382,13 @@
fprintf(fp, "%%PositionLeft,ForceLeft,PositionRight,ForceRight,Time(ms),Direction,CycleNum\n");
#endif
// We are go-times!
- g_current_mode=MODE_AUTOMATIC;
+ collect_data=true;
}
- if( g_current_mode==MODE_AUTOMATIC && g_current_cycle >= g_num_cycles)
+ if( collect_data && g_current_cycle >= g_num_cycles)
{
// STOOOOOOOOOP
- g_current_mode=MODE_NULL;
+ collect_data=false;
#ifdef USE_SD_CARD
// Close the file
fclose(fp);
@@ -634,7 +396,7 @@
#endif
}
- // This section of code should run whenever there is free time to save datas
+ // This section of code should run whenever there is free time to print to the screen
#ifdef USE_SD_CARD
if(fp != NULL) {
// Only write to SD if there is a valid file handle
@@ -643,163 +405,20 @@
led3 = 0;
}
#else
- //Warning, this is a big fat stream of data, 1 minute is approx 3MB
- //I certainly recommend using the SD card.
Buffer.dumpBufferToSerial();
#endif
- if(g_current_mode!=MODE_NULL && AuxSerialTimer.read_us()>1000){
+ if(AuxSerialTimer.read_ms()>100 && collect_data){
//Send some extra data for GUI purposes
- #ifdef ROD_IS_RIGHT
- float angle=0,angledot=0,angledotdot=0;
- if(angledotdot_count>0){
- angle =float(angle_sum )/angle_count;
- angledot =float(angledot_sum )/angledot_count;
- angledotdot=float(angledotdot_sum)/angledotdot_count;
- }
- angle_sum=0;angle_count=0;
- angledot_sum=0;angledot_count=0;
- angledotdot_sum=0;angledotdot_count=0;
- #else
- crunching_data_flag=true;
- recent_pos.least_squares_3rd(coeffs);
- crunching_data_flag=false;
- float x=g_current_trajectory_time;
- int angle =coeffs[0]+coeffs[1]*x+coeffs[2]*x*x+coeffs[3]*x*x*x;
- int angledot = coeffs[1]+2*coeffs[2]*x+3*coeffs[3]*x*x ;
- int angledotdot= 2*coeffs[2] +6*coeffs[3]*x ;
- #endif
-
-
-
- float pot_open=0.75;
- float pot_closed=0.48;
-
- float percent=0.0;
- //Average the pot a bunch, it's NOISY
- for(int i=0;i<100;i++){
- percent+=AinJoystickFwdBk.read();
- }
- percent=percent/100.0;
- percent=(pot_open-percent)/(pot_open-pot_closed);
-
- // The 'pulling' of the trigger corresponds to open/closed
- int potread = percent*(CLOSED_SERVO_ANGLE_LEFT -OPEN_SERVO_ANGLE_LEFT )+OPEN_SERVO_ANGLE_LEFT ;
-
- //angle=left_servo_measured;
-
- //if (!calibrateDone){
- // wait_ms(2);
- //}
- int loadcell=adc.read();
-
- if (!calibrateDone){
- g_command_corrected = 2800;
- // Begin Calibration of load cells
- loadCellTotal += (long long) loadcell;
- calibCntr ++;
- if (calibCntr == CALIBRATION_READS){
- g_loadCellZero = float(loadCellTotal)/float(CALIBRATION_READS);
- calibrateDone = true;
- }
- }
-
-
- #ifdef CALIBRATE_TIME_NOW
- if (calibrateDone){
- g_command_corrected=potread;
- int loadcell_offset = loadcell - g_loadCellZero;
- if(loadcell_offset < 0){
- loadcell_offset = 0;
- }
- printf("%6.3f,%0.0f,%2.3f,%2.3f,%d\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell_offset);
-
- }
- #else
- float loadcell_offset = float(loadcell) - g_loadCellZero;
- if(loadcell_offset < 0.0){
- loadcell_offset = 0.0;
- }
- // Convert Load cell read into Newtons
- forceInNewtons = ((loadcell_offset)*g_loadCellSlopeCalibration)*g_gravity;
- // Code to determine if we should toggle the variable of "g_we_are_grasping"
- if(!g_we_are_grasping && forceInNewtons>g_forceAboveFirstTouch && angledot<velocity_threshold){
- //Grasp is starting
- g_we_are_grasping=true;
- // Set the position and time at first touch
- g_masterPositionFirstTouch = potread;
- g_slavePositionFirstTouch = angle;
- g_timeAtFirstTouch = mytimer.read_us()/1000.0;
- for(int i=0;i<NUMBER_OF_TISSUES;i++){
- g_error_norm[i]=0.0;
- }
- }
- if(g_we_are_grasping && potread > 2400 && (mytimer.read_us()/1000.0) > (g_timeAtFirstTouch + 200.0)) {//angledot>-velocity_threshold){
- //Grasp is over
- g_error_norm[0] = 0;
- g_error_norm[1] = 0;
- integral_err = 0.0;
- g_we_are_grasping=false;
- }
- float alpha=0;
- std::string sstr;
- if(g_we_are_grasping){
- //D_x = [thetadotdot,thetadot,theta,theta^2,theta^3];
- 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));
- 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));
-
-
- float offset1 = 100000;
- float offset2 = 300000;
- int tissue_id=0;
- 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;}
- 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;}
- alpha += 0.15;
- if (alpha > 1.0){alpha = 1.0;}
- float force_err=loadcell_offset-g_thresh_force[tissue_id];
- integral_err = integral_err + force_err;
- float current_time = mytimer.read_us()/1000.0;
- float dt = 39.0; //Change to a real approach later
- float kp = 10.0/0.1e6;
- float ki = 10.0/0.1e8;
- if (tissue_id == 0){kp=10.0/0.1e6;}
- if (tissue_id == 1){kp=100.0/0.1e6;}
- // Collaborative Mode
- //g_command_corrected=(1-alpha)*potread+alpha*(kp*force_err+ki*integral_err*dt+angle);
- // Fully Autonomous Mode
- //g_command_corrected=(kp*force_err+angle);
- // Fully Manual Mode
- g_command_corrected = potread;
- }else{
- g_command_corrected=potread;
- }
-
- //std::vector<int> data_in=<angle, angledot, angledotdot, loadcell, potread>
-
- if (!calibrateDone){
- g_command_corrected = 2800;
- }
- //printf("Data:,%6.3f,%6.3f,%3.4f,%6.3f\n",(float(g_slavePositionFirstTouch)-angle),forceInNewtons,float(g_slavePositionFirstTouch),forceInNewtons);
- printf("Data:,%6.3f,%6.3f,%6.3f,%6.3f,%6.3f\n",mytimer.read_us()/1000.0,forceInNewtons,(float(g_slavePositionFirstTouch)-angle),angledot,angledotdot);
-
-
- //printf("Data:,%6.3f,%6.3f,%3.4f,%d\n",mytimer.read_us()/1000.0,g_loadCellZero,forceInNewtons,adc.read());
-
-
- #endif
-
- //printf("%s",recent_pos.get_string().c_str());
-// printf("<%d,%d,%d,%d,%d,%d,%d> ",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos,
-// tempSpindleData.myServoData[LEFT_SERVO_INDEX].force,
-// tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos,
-// tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force,
-// tempSpindleData.time,
-// tempSpindleData.direction,
-// tempSpindleData.cycle);
-// printf(" %dus", worst_latency);
-// worst_latency=0;
-// printf(" %0.0fbps\n", bits_received*1000.0/AuxSerialTimer.read_ms());
-// bits_received=0;
+ printf("<%d,%d,%d,%d,%d,%d,%d> ",tempSpindleData.myServoData[LEFT_SERVO_INDEX].pos,
+ tempSpindleData.myServoData[LEFT_SERVO_INDEX].force,
+ tempSpindleData.myServoData[RIGHT_SERVO_INDEX].pos,
+ tempSpindleData.myServoData[RIGHT_SERVO_INDEX].force,
+ tempSpindleData.time,
+ tempSpindleData.direction,
+ tempSpindleData.cycle);
+ printf(" %dus\n", worst_latency);
+ worst_latency=0;
AuxSerialTimer.reset();
}