initExoVars.cpp

00001 #include "mbed.h"
00002 #include "Encoder.h"
00003 #include "initExoVars.h"
00004 #include "HipControl.h"
00005 #include "knee.h"
00006 #include "FSM.h"
00007 #include "localRead.h"
00008 
00009 //#include "MM_gait.h"
00010 //#include "constants.h"
00011 
00012 /********************************
00013 *  Initializing various I/O pins!
00014 ********************************/
00015 DigitalOut mbedLED1(LED1);
00016 DigitalOut mbedLED2(LED2);
00017 DigitalOut mbedLED3(LED3);
00018 DigitalOut motorLED(LED4);
00019 
00020 SPI dataBedSPI(p11, p12, p13); // mosi, miso, sclk
00021 DigitalOut DB_cs(p14);
00022 
00023 Serial pc(USBTX, USBRX);
00024 //StepperDriver Stepper(p18,p5,p6,p7);
00025 
00026 // Setup stuff
00027 // absolutes
00028 Encoder encoder_L(p5,p6,p7,p30);//MOSI,MISO,SCLK,SS
00029 Encoder encoder_R(p5,p6,p7,p29);
00030 HipControl hip_L(p23,p24);
00031 HipControl hip_R(p21,p22);
00032 //Knee knee_L(p18,p5,p6,p7);
00033 //Knee knee_R(p19,p5,p6,p7);
00034 Knee knee_L(p5,p6,p7,p18);
00035 Knee knee_R(p5,p6,p7,p19);
00036 
00037 FSM fsm;
00038 
00039 // Databed comm stuff
00040 Serial DB_serial(p9, p10);
00041 DigitalIn FeedDataDuino(p9);
00042 
00043 /*******************************
00044 * END OF I/O Initialization
00045 *******************************/
00046 
00047 /*******************************
00048 * Global vars
00049 *******************************/
00050 // global timers
00051 Timer sysClk;     // system clock (primarily for datalogging)
00052 Timer tState;   // time in FSM state
00053 Timer t_HCl;  // time when high current begins
00054 Timer t_HCr;
00055 Timer t_BetweenStepCommand;
00056 
00057 //Control Variables
00058 // Control variables
00059 // Gain Scheduling!
00060 float Kp_Swing        = .02;
00061 float Kp_Stance       = .02;
00062 const float Kp_DoubleStance = .02;//.01;
00063 const float Kp_Standing     = .02;//.01; //0.0025
00064 const float Kp_StandUp      = .01;
00065 const float Kp_SitDown      = .01;
00066 const float Kp_Seated      = .01;
00067 float Kd_Swing=0.002;
00068 float Kd_Stance=0.002;
00069 
00070 const float Kp=.04;
00071 const float Kp0=.001;
00072 float Kd=.002;
00073 const float sat=.2;
00074 const int blend_thresh=250; //samples before we switch to the true trajectory
00075 const float Kp_mag=(Kp0-Kp)/2;
00076 const float Kp_freq=PI/blend_thresh;
00077 const float Kp_offset=(Kp0+Kp)/2;
00078 
00079 float TimeSinceLastStep;
00080 //encoder setup
00081 //float stand_adjust= 10; //positive is more upright.Default 10
00082 //const float zero_ang_L=220+10;//approximate point where the pilot should be standing straight
00083 //const float zero_ang_R=40+10;//approximate point where the pilot should be standing straight
00084 
00085 /** VERIFY THESE VALUES BEFORE RUNNING CODE ON EXO
00086 * USING ERRONEOUS OFFSETS MAY CAUSE PERMANENT DAMAGE!!! */
00087 //const float zero_ang_L=223.83;//approximate point where the pilot should be standing straight
00088 //const float zero_ang_R=-176.03;//approximate point where the pilot should be standing straight
00089 //const float zero_ang_L=168.24;//approximate point where the pilot should be standing straight
00090 //const float zero_ang_R=-128.12;//approximate point where the pilot should be standing straight
00091 
00092 //const float _enc_high=120; // Hard stop max accepable angle for encoder
00093 //const float _enc_low=-40; // Hard stop min accepable angle for encoder
00094 
00095 //PARAMETER ADJUSTMENT - Standing Angle
00096 //Steve = 10
00097 float stand_adjust=10; //positive is more upright//Steve=10 deg
00098 //Person 1
00099 //const float stand_adjust=10;
00100 //Person 2
00101 //const float stand_adjust=10;
00102 
00103 //zero_enc_offset_X is the offset for a particular encoder. Must be tuned for an individual encoder based on assembly.
00104 // Offsets for Exo #1. These are related to individual encoder and should never be changed.
00105 //float zero_enc_offset_L = 223.83;
00106 // zero_enc_offset_R should be negative
00107 //float zero_enc_offset_R = -176.4;
00108 //float bob_dole=-176.4;
00109 // Offsets for Test Rig
00110 //float zero_enc_offset_L = 205.24;
00111 // zero_enc_offset_R should be negative
00112 //float zero_enc_offset_R = -135.17;
00113 //float bob_dole=-135.17;
00114 float zero_enc_offset_L;//=LeftCalib;
00115 float zero_enc_offset_R;//=RightCalib;
00116 //float bob_dole;//=RightCalib;
00117 
00118 
00119 //Offset for Test Rig
00120 //const float zero_enc_offset_L = 205.24;
00121 // zero_enc_offset_R should be negative
00122 //const float zero_enc_offset_R = -135.17;
00123 
00124 
00125 //zero_ang_X is the zero angle based on the mechanical assembly of the hip and the standing angle parameters of the individual user.
00126 //const float zero_ang_L=zero_enc_offset_L+stand_adjust;
00127 //const float zero_ang_R=zero_enc_offset_R+stand_adjust;
00128 float zero_ang_L;
00129 float zero_ang_R;
00130 
00131 // Upper and lower limit on Encoder
00132 // TODO(Cashdollar): These may need to be tuned. Window is 160 deg but might not start at 120.
00133 const float _enc_high=122+stand_adjust; // Hard stop max accepable angle for encoder
00134 const float _enc_low=-38+stand_adjust; // Hard stop min accepable angle for encoder
00135 
00136 
00137 MMgait_t mm_gait_params;
00138 Brad_poly_gait_t gait_params;
00139 //MMSwing swing(mm_gait_params);
00140 //MMStance stance(mm_gait_params);
00141 //MMSwing swing;
00142 //MMStance stance;
00143 BradPolySwing swing;
00144 BradPolyStance stance;
00145 MMFSSwing fsSwing;
00146 MMFSStance fsStance;
00147 MMFTG ftgSwing;
00148 MMFTG ftgStance;
00149 LinearBlend blend_left;
00150 LinearBlend blend_right;
00151 
00152 
00153 void initializeExoIOs()
00154 {
00155     // Putting various comm pins high
00156     DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)
00157 
00158     localRead();
00159     zero_enc_offset_L=LeftCalib;
00160     zero_enc_offset_R=RightCalib;
00161     
00162     zero_ang_L=zero_enc_offset_L+stand_adjust;
00163     zero_ang_R=zero_enc_offset_R+stand_adjust;
00164 
00165     // initialize both knees
00166     knee_L.init();
00167     knee_R.init();
00168     //Setting pwm frequencies to 20kHz
00169 
00170     hip_L.off();
00171     hip_R.off();
00172     hip_R.flip();
00173     encoder_L.init(zero_ang_L);
00174     encoder_R.init(zero_ang_R);
00175     encoder_R.flip();
00176     // display debugging info for both knees
00177     //knee_L.debug();
00178     //knee_R.debug();
00179     encoder_L.ranges(_enc_low, _enc_high);
00180     encoder_R.ranges(_enc_low, _enc_high);
00181     encoder_L.rangeCheck();
00182     encoder_R.rangeCheck();
00183     fsm.init();
00184 
00185 
00186     dataBedSPI.frequency(10000000);
00187 
00188 
00189     mm_gait_params.time_steps=900;
00190     mm_gait_params.peak_time=416;
00191     mm_gait_params.stance_start=10;
00192     mm_gait_params.stance_end=-22;
00193     mm_gait_params.max_angle=30;
00194     mm_gait_params.max_fs_angle=mm_gait_params.max_angle*1.16;
00195 
00196     gait_params.time_steps=900;
00197     gait_params.peak_time=416;
00198     gait_params.stance_start=10;
00199     gait_params.stance_end=-22;
00200     gait_params.max_angle=30;
00201     gait_params.max_fs_angle=mm_gait_params.max_angle*1.16;
00202 
00203     swing.set(gait_params);
00204     stance.set(gait_params);
00205     fsSwing.set(mm_gait_params);
00206     fsStance.set(mm_gait_params);
00207     ftgSwing.set(mm_gait_params);
00208     ftgStance.set(mm_gait_params);
00209     //pc.printf("%Mem: %d \r\n", AvailableMemory());
00210 
00211 
00212     // print each knee status
00213     pc.printf("knee_L: %x, knee_R:%x\r\n", knee_L.status(), knee_R.status());
00214 
00215 }
00216