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
Diff: main.cpp
- Revision:
- 10:5c0c9c647090
- Parent:
- 9:f26a91863e3a
- Child:
- 11:53408545bbb4
--- a/main.cpp Tue May 12 17:04:31 2015 +0000
+++ b/main.cpp Tue May 12 20:04:59 2015 +0000
@@ -94,7 +94,7 @@
#define NUMBER_OF_TISSUES 2
float g_error_norm[NUMBER_OF_TISSUES];
bool g_we_are_grasping=false;
-#define CALIBRATION_READS 100.0
+#define CALIBRATION_READS 200.0
float g_calibration_offset = 8600000.0;
// Values at first touch
@@ -104,23 +104,34 @@
float g_timeAtFirstTouch;
///////////Magic numbers courtesy Rod///////////////
-float Phi1[5]={-8.02086003501975e-08,
- 1.55286905458007e-05,
- 0.00795344249784777,
- 8.23733045077119,
- -0.00299236282304311};
+//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
+float Phi1[5] = {-1.80212053214826e-08,
+ 1.69579390650964e-06,
+ 0.000592679062823746,
+ 0.624774980172511,
+ -0.000283294192960159};
-float Phi2[5]={-1.24436831310503e-08,
- 1.23673348605010e-05,
- 0.00652545188345528,
- 6.75893262890734,
- -0.00228098997419065};
-
+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 velocty_threshold=-0.01;
+float velocity_threshold=-0.01;
-float g_thresh_force[NUMBER_OF_TISSUES]={8.80e6,8.75e6};
+float g_thresh_force[NUMBER_OF_TISSUES]={0.16e6,0.12e6};
///////////Magic numbers courtesy Rod///////////////
int g_command_corrected;
@@ -128,11 +139,11 @@
// These are for load cell initialization of the offset
float g_loadCellZero = 8600000.0;
bool calibrateDone = false;
-long calibrateTotal = 8600;
+//long calibrateTotal = 8600;
int calibCntr = 0;
-int g_loadCellOffset = 100000;
-int g_threshOffset1 = 0.2e6;
-int g_threshOffset2 = 0.15e6;
+//int g_loadCellOffset = 100000;
+//int g_threshOffset1 = 0.2e6;
+//int g_threshOffset2 = 0.15e6;
long long loadCellTotal = 0;
Timer mytimer;
@@ -461,7 +472,7 @@
//}
}
-// Calibrate Load Cell Function
+// Calibrate Load Cell Function (NOT USING THIS ONE!!!)
void calibrateLoadCell() {
long long loadCellTotal = 0;
int intermediateValue;
@@ -470,12 +481,12 @@
//pc.printf("Load Cell Read: %i\n",intermediateValue);
loadCellTotal+= intermediateValue;
wait_ms(2);
- pc.printf("We are calibrating load cells...\n");
+ //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;
+ //g_thresh_force[0]=g_threshOffset1+g_calibration_offset;
+ //g_thresh_force[1]=g_threshOffset2+g_calibration_offset;
calibrateDone = true;
}
@@ -663,37 +674,25 @@
// 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();
- //pc.printf("Load Cell Read: %d\n",loadcell);
-// if (!calibrateDone){
-// calibrateLoadCell();
-// }
+
if (!calibrateDone){
g_command_corrected = 2800;
// Begin Calibration of load cells
- // Calibrate Load Cells (Does this need to get moved somewhere else?)
- //calibrateLoadCell();
- // These are for load cell initialization of the offset
- //calibrateTotal = (calibrateTotal + loadcell/1000.0)/2.0;
-
- //pc.printf("Load Cell Read: %d\n",loadcell);
loadCellTotal += (long long) loadcell;
calibCntr ++;
if (calibCntr == CALIBRATION_READS){
g_loadCellZero = float(loadCellTotal)/float(CALIBRATION_READS);
- //Update values for thresholding based on calibration
- g_thresh_force[0]=g_threshOffset1+g_loadCellZero;
- g_thresh_force[1]=g_threshOffset2+g_loadCellZero;
calibrateDone = true;
}
}
-
#ifdef CALIBRATE_TIME_NOW
if (calibrateDone){
g_command_corrected=potread;
@@ -703,10 +702,14 @@
}
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;
+ }
// Code to determine if we should toggle the variable of "g_we_are_grasping"
- if(!g_we_are_grasping && loadcell>g_loadCellZero+g_forceAboveFirstTouch && angledot<velocty_threshold){
+ if(!g_we_are_grasping && loadcell_offset>g_forceAboveFirstTouch && angledot<velocity_threshold){
//Grasp is starting
g_we_are_grasping=true;
// Set the position and time at first touch
@@ -716,7 +719,7 @@
g_error_norm[i]=0.0;
}
}
- if(g_we_are_grasping && potread > g_masterPositionFirstTouch && (mytimer.read_us()/1000.0) > (g_timeAtFirstTouch + 200.0)) {//angledot>-velocty_threshold){
+ 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;
@@ -726,8 +729,8 @@
std::string sstr;
if(g_we_are_grasping){
//D_x = [thetadotdot,thetadot,theta,theta^2,theta^3];
- 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));
- 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));
+ 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;
@@ -736,7 +739,7 @@
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;}
- float force_err=loadcell-g_thresh_force[tissue_id];
+ float force_err=loadcell_offset-g_thresh_force[tissue_id];
float k=20/0.1e6;
g_command_corrected=(1-alpha)*potread+alpha*(k*force_err+angle);
}else{
@@ -744,15 +747,14 @@
}
//std::vector<int> data_in=<angle, angledot, angledotdot, loadcell, potread>
- int loadcell_offset = loadcell - g_loadCellZero;
- if(loadcell_offset < 0){
- loadcell_offset = 0;
- }
+
if (!calibrateDone){
g_command_corrected = 2800;
}
+ printf("Data:,%6.3f,%6.3f,%2.3f,%s\n",mytimer.read_us()/1000.0,loadcell_offset,alpha,sstr.c_str());
- printf("Data:,%6.3f,%0.0f,% 2.3f,% 2.3f,%d,%d,%0.0f,%12.3f,%2.3f,%s\n",mytimer.read_us()/1000.0,angle, angledot, angledotdot, loadcell, potread,g_error_norm[0],g_loadCellZero,alpha,sstr.c_str());
+ //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());
+
#endif
//printf("%s",recent_pos.get_string().c_str());