File content as of revision 13:268f4564a809:

#include "mbed.h"
#include "Encoder.h"
#include "initExoVars.h"
#include "HipControl.h"
#include "knee.h"
#include "FSM.h"
#include "localRead.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!
float Kp_Swing        = .02;
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;
float Kd_Swing=0.002;
float Kd_Stance=0.002;

const float Kp=.04;
const float Kp0=.001;
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.
//float zero_enc_offset_L = 223.83;
// zero_enc_offset_R should be negative
//float zero_enc_offset_R = -176.4;
//float bob_dole=-176.4;
// Offsets for Test Rig
//float zero_enc_offset_L = 205.24;
// zero_enc_offset_R should be negative
//float zero_enc_offset_R = -135.17;
//float bob_dole=-135.17;
float zero_enc_offset_L;//=LeftCalib;
float zero_enc_offset_R;//=RightCalib;
//float bob_dole;//=RightCalib;


//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;
float zero_ang_L;
float zero_ang_R;

// 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;
Brad_poly_gait_t gait_params;
//MMSwing swing(mm_gait_params);
//MMStance stance(mm_gait_params);
//MMSwing swing;
//MMStance stance;
BradPolySwing swing;
BradPolyStance stance;
MMFSSwing fsSwing;
MMFSStance fsStance;
MMFTG ftgSwing;
MMFTG ftgStance;
LinearBlend blend_left;
LinearBlend blend_right;


void initializeExoIOs()
{
    // Putting various comm pins high
    DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)

    localRead();
    zero_enc_offset_L=LeftCalib;
    zero_enc_offset_R=RightCalib;
    
    zero_ang_L=zero_enc_offset_L+stand_adjust;
    zero_ang_R=zero_enc_offset_R+stand_adjust;

    // 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();


    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;
    mm_gait_params.max_fs_angle=mm_gait_params.max_angle*1.16;

    gait_params.time_steps=900;
    gait_params.peak_time=416;
    gait_params.stance_start=10;
    gait_params.stance_end=-22;
    gait_params.max_angle=30;
    gait_params.max_fs_angle=mm_gait_params.max_angle*1.16;

    swing.set(gait_params);
    stance.set(gait_params);
    fsSwing.set(mm_gait_params);
    fsStance.set(mm_gait_params);
    ftgSwing.set(mm_gait_params);
    ftgStance.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());

}