File content as of revision 6:87ad618621a4:

#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!
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;

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=0; //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;


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

/*void readCalib()
{
    char A[10];
    LocalFileSystem local("local");               // Create the local filesystem under the name "local"
    FILE *set = fopen("/local/calib.txt", "r");  // Open "setup.txt" on the local file system for read
    fscanf(set,"%s %f",A,&zero_enc_offset_L); // read offset
    fscanf(set,"%s %f",A,&zero_enc_offset_R); // read offset
    fclose(set);
    wait(1); //TODO(Cashdollar): reduce this time after testing
    pc.printf("\r\nLeft Offset is: %f, \r\nRight Offset is: %f\r\n", zero_enc_offset_L, zero_enc_offset_R);//, zero_enc_offset_R);
    wait(1); //TODO(Cashdollar): Delete after testing, this is to make it easier to see what is happening
}*/

void initializeExoIOs()
{
    pc.printf("\r\n entered initexoios");
    // Putting various comm pins high
    DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)
    //pc.baud(921600);
    //pc.baud(921600);
    //readCalib();
    zero_ang_L=zero_enc_offset_L+stand_adjust;
    zero_ang_R=zero_enc_offset_R+stand_adjust;
    pc.printf("\r\nzero ang assigned");

    // initialize both knees
    knee_L.init();
    knee_R.init();
    //Setting pwm frequencies to 20kHz
    pc.printf("\r\n knees init");

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

    swing.set(mm_gait_params);
    stance.set(mm_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());


}