Diff: initExoVars.cpp

Revision:

0:cd5ad8e47eb8

Child:

1:72e0bd01c8e0

Child:

2:650a015179b9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/initExoVars.cpp	Wed Jun 24 19:24:08 2015 +0000
@@ -0,0 +1,177 @@
+#include "mbed.h"
+#include "Encoder.h"
+#include "initExoVars.h"
+#include "HipControl.h"
+#include "knee.h"
+#include "FSM.h"
+//#include "MM_gait.h"
+//#include "constants.h"
+
+/********************************
+*  Initializing various I/O pins!
+********************************/
+DigitalOut mbedLED1(LED1);
+DigitalOut mbedLED2(LED2);
+DigitalOut mbedLED3(LED3);
+DigitalOut motorLED(LED4);
+
+SPI dataBedSPI(p11, p12, p13); // mosi, miso, sclk
+DigitalOut DB_cs(p14);
+
+Serial pc(USBTX, USBRX);
+//StepperDriver Stepper(p18,p5,p6,p7);
+
+// Setup stuff
+// absolutes
+Encoder encoder_L(p5,p6,p7,p30);//MOSI,MISO,SCLK,SS
+Encoder encoder_R(p5,p6,p7,p29);
+HipControl hip_L(p23,p24);
+HipControl hip_R(p21,p22);
+//Knee knee_L(p18,p5,p6,p7);
+//Knee knee_R(p19,p5,p6,p7);
+Knee knee_L(p5,p6,p7,p18);
+Knee knee_R(p5,p6,p7,p19);
+
+FSM fsm;
+
+// Databed comm stuff
+Serial DB_serial(p9, p10);
+DigitalIn FeedDataDuino(p9);
+
+/*******************************
+* END OF I/O Initialization
+*******************************/
+
+/*******************************
+* Global vars
+*******************************/
+// global timers
+Timer sysClk;     // system clock (primarily for datalogging)
+Timer tState;   // time in FSM state
+Timer t_HCl;  // time when high current begins
+Timer t_HCr;
+Timer t_BetweenStepCommand;
+
+//Control Variables
+// Control variables
+// Gain Scheduling!
+const float Kp_Swing        = .02;
+const float Kp_Stance       = .02;
+const float Kp_DoubleStance = .02;//.01;
+const float Kp_Standing     = .02;//.01; //0.0025
+const float Kp_StandUp      = .01;
+const float Kp_SitDown      = .01;
+const float Kp_Seated      = .01;
+
+const float Kp=.04;
+const float Kp0=.001;
+const float Kd=.002;
+const float sat=.2;
+const int blend_thresh=250; //samples before we switch to the true trajectory
+const float Kp_mag=(Kp0-Kp)/2;
+const float Kp_freq=PI/blend_thresh;
+const float Kp_offset=(Kp0+Kp)/2;
+
+float TimeSinceLastStep;
+//encoder setup
+//float stand_adjust= 10; //positive is more upright.Default 10
+//const float zero_ang_L=220+10;//approximate point where the pilot should be standing straight
+//const float zero_ang_R=40+10;//approximate point where the pilot should be standing straight
+
+/** VERIFY THESE VALUES BEFORE RUNNING CODE ON EXO
+* USING ERRONEOUS OFFSETS MAY CAUSE PERMANENT DAMAGE!!! */
+//const float zero_ang_L=223.83;//approximate point where the pilot should be standing straight
+//const float zero_ang_R=-176.03;//approximate point where the pilot should be standing straight
+//const float zero_ang_L=168.24;//approximate point where the pilot should be standing straight
+//const float zero_ang_R=-128.12;//approximate point where the pilot should be standing straight
+
+//const float _enc_high=120; // Hard stop max accepable angle for encoder
+//const float _enc_low=-40; // Hard stop min accepable angle for encoder
+
+//PARAMETER ADJUSTMENT - Standing Angle
+//Steve = 10
+float stand_adjust=10; //positive is more upright//Steve=10 deg
+//Person 1
+//const float stand_adjust=10;
+//Person 2
+//const float stand_adjust=10;
+ 
+//zero_enc_offset_X is the offset for a particular encoder. Must be tuned for an individual encoder based on assembly.
+// Offsets for Exo #1. These are related to individual encoder and should never be changed.
+//const float zero_enc_offset_L = 223.83;
+// zero_enc_offset_R should be negative
+//const float zero_enc_offset_R = -176.4;
+// Offsets for Exo #2
+//const float zero_enc_offset_L = 168.24;
+// zero_enc_offset_R should be negative
+//const float zero_enc_offset_R = -128.12;
+//Offset for Test Rig
+const float zero_enc_offset_L = 205.24;
+// zero_enc_offset_R should be negative
+const float zero_enc_offset_R = -135.17;
+ 
+//zero_ang_X is the zero angle based on the mechanical assembly of the hip and the standing angle parameters of the individual user.
+const float zero_ang_L=zero_enc_offset_L+stand_adjust;
+const float zero_ang_R=zero_enc_offset_R+stand_adjust;
+ 
+// Upper and lower limit on Encoder
+// TODO(Cashdollar): These may need to be tuned. Window is 160 deg but might not start at 120.
+const float _enc_high=122+stand_adjust; // Hard stop max accepable angle for encoder
+const float _enc_low=-38+stand_adjust; // Hard stop min accepable angle for encoder
+
+
+MMgait_t mm_gait_params;
+//MMSwing swing(mm_gait_params);
+//MMStance stance(mm_gait_params);
+MMSwing swing;
+MMStance stance;
+
+void initializeExoIOs()
+{
+    // Putting various comm pins high
+    DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)
+    pc.baud(921600); //pc.baud(115200);//460800
+    //pc.baud(230400);
+
+    // initialize both knees
+    knee_L.init();
+    knee_R.init();
+//Setting pwm frequencies to 20kHz
+
+    hip_L.off();
+    hip_R.off();
+    hip_R.flip();
+    encoder_L.init(zero_ang_L);
+    encoder_R.init(zero_ang_R);
+    encoder_R.flip();
+    // display debugging info for both knees
+    //knee_L.debug();
+    //knee_R.debug();
+    encoder_L.ranges(_enc_low, _enc_high);
+    encoder_R.ranges(_enc_low, _enc_high);
+    encoder_L.rangeCheck();
+    encoder_R.rangeCheck();
+    fsm.init();
+    pc.printf("Test2 \r\n");
+
+
+    dataBedSPI.frequency(10000000);
+
+
+    mm_gait_params.time_steps=900;
+    mm_gait_params.peak_time=416;
+    mm_gait_params.stance_start=10;
+    mm_gait_params.stance_end=-22;
+    mm_gait_params.max_angle=30;
+
+    swing.set(mm_gait_params);
+    stance.set(mm_gait_params);
+    //pc.printf("%Mem: %d \r\n", AvailableMemory());
+
+
+    // print each knee status
+
+    pc.printf("knee_L: %x, knee_R:%x\r\n,", knee_L.status(), knee_R.status());
+
+
+}