Nathaniel Honka / initExoVars

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.

Fork of initExoVars by HEL's Angels

initExoVars.cpp@1:72e0bd01c8e0, 2015-06-26 (annotated)

Committer:
perr1940
Date:
Fri Jun 26 16:28:02 2015 +0000
Revision:
1:72e0bd01c8e0
Parent:
0:cd5ad8e47eb8 
added new FTG and first step generators

Who changed what in which revision?

UserRevisionLine numberNew contents of line
perr1940 0:cd5ad8e47eb8 1 #include "mbed.h"
perr1940 0:cd5ad8e47eb8 2 #include "Encoder.h"
perr1940 0:cd5ad8e47eb8 3 #include "initExoVars.h"
perr1940 0:cd5ad8e47eb8 4 #include "HipControl.h"
perr1940 0:cd5ad8e47eb8 5 #include "knee.h"
perr1940 0:cd5ad8e47eb8 6 #include "FSM.h"
perr1940 0:cd5ad8e47eb8 7 //#include "MM_gait.h"
perr1940 0:cd5ad8e47eb8 8 //#include "constants.h"
perr1940 0:cd5ad8e47eb8 9
perr1940 0:cd5ad8e47eb8 10 /********************************
perr1940 0:cd5ad8e47eb8 11 * Initializing various I/O pins!
perr1940 0:cd5ad8e47eb8 12 ********************************/
perr1940 0:cd5ad8e47eb8 13 DigitalOut mbedLED1(LED1);
perr1940 0:cd5ad8e47eb8 14 DigitalOut mbedLED2(LED2);
perr1940 0:cd5ad8e47eb8 15 DigitalOut mbedLED3(LED3);
perr1940 0:cd5ad8e47eb8 16 DigitalOut motorLED(LED4);
perr1940 0:cd5ad8e47eb8 17
perr1940 0:cd5ad8e47eb8 18 SPI dataBedSPI(p11, p12, p13); // mosi, miso, sclk
perr1940 0:cd5ad8e47eb8 19 DigitalOut DB_cs(p14);
perr1940 0:cd5ad8e47eb8 20
perr1940 0:cd5ad8e47eb8 21 Serial pc(USBTX, USBRX);
perr1940 0:cd5ad8e47eb8 22 //StepperDriver Stepper(p18,p5,p6,p7);
perr1940 0:cd5ad8e47eb8 23
perr1940 0:cd5ad8e47eb8 24 // Setup stuff
perr1940 0:cd5ad8e47eb8 25 // absolutes
perr1940 0:cd5ad8e47eb8 26 Encoder encoder_L(p5,p6,p7,p30);//MOSI,MISO,SCLK,SS
perr1940 0:cd5ad8e47eb8 27 Encoder encoder_R(p5,p6,p7,p29);
perr1940 0:cd5ad8e47eb8 28 HipControl hip_L(p23,p24);
perr1940 0:cd5ad8e47eb8 29 HipControl hip_R(p21,p22);
perr1940 0:cd5ad8e47eb8 30 //Knee knee_L(p18,p5,p6,p7);
perr1940 0:cd5ad8e47eb8 31 //Knee knee_R(p19,p5,p6,p7);
perr1940 0:cd5ad8e47eb8 32 Knee knee_L(p5,p6,p7,p18);
perr1940 0:cd5ad8e47eb8 33 Knee knee_R(p5,p6,p7,p19);
perr1940 0:cd5ad8e47eb8 34
perr1940 0:cd5ad8e47eb8 35 FSM fsm;
perr1940 0:cd5ad8e47eb8 36
perr1940 0:cd5ad8e47eb8 37 // Databed comm stuff
perr1940 0:cd5ad8e47eb8 38 Serial DB_serial(p9, p10);
perr1940 0:cd5ad8e47eb8 39 DigitalIn FeedDataDuino(p9);
perr1940 0:cd5ad8e47eb8 40
perr1940 0:cd5ad8e47eb8 41 /*******************************
perr1940 0:cd5ad8e47eb8 42 * END OF I/O Initialization
perr1940 0:cd5ad8e47eb8 43 *******************************/
perr1940 0:cd5ad8e47eb8 44
perr1940 0:cd5ad8e47eb8 45 /*******************************
perr1940 0:cd5ad8e47eb8 46 * Global vars
perr1940 0:cd5ad8e47eb8 47 *******************************/
perr1940 0:cd5ad8e47eb8 48 // global timers
perr1940 0:cd5ad8e47eb8 49 Timer sysClk; // system clock (primarily for datalogging)
perr1940 0:cd5ad8e47eb8 50 Timer tState; // time in FSM state
perr1940 0:cd5ad8e47eb8 51 Timer t_HCl; // time when high current begins
perr1940 0:cd5ad8e47eb8 52 Timer t_HCr;
perr1940 0:cd5ad8e47eb8 53 Timer t_BetweenStepCommand;
perr1940 0:cd5ad8e47eb8 54
perr1940 0:cd5ad8e47eb8 55 //Control Variables
perr1940 0:cd5ad8e47eb8 56 // Control variables
perr1940 0:cd5ad8e47eb8 57 // Gain Scheduling!
perr1940 0:cd5ad8e47eb8 58 const float Kp_Swing = .02;
perr1940 0:cd5ad8e47eb8 59 const float Kp_Stance = .02;
perr1940 0:cd5ad8e47eb8 60 const float Kp_DoubleStance = .02;//.01;
perr1940 0:cd5ad8e47eb8 61 const float Kp_Standing = .02;//.01; //0.0025
perr1940 0:cd5ad8e47eb8 62 const float Kp_StandUp = .01;
perr1940 0:cd5ad8e47eb8 63 const float Kp_SitDown = .01;
perr1940 0:cd5ad8e47eb8 64 const float Kp_Seated = .01;
perr1940 0:cd5ad8e47eb8 65
perr1940 0:cd5ad8e47eb8 66 const float Kp=.04;
perr1940 0:cd5ad8e47eb8 67 const float Kp0=.001;
perr1940 0:cd5ad8e47eb8 68 const float Kd=.002;
perr1940 0:cd5ad8e47eb8 69 const float sat=.2;
perr1940 0:cd5ad8e47eb8 70 const int blend_thresh=250; //samples before we switch to the true trajectory
perr1940 0:cd5ad8e47eb8 71 const float Kp_mag=(Kp0-Kp)/2;
perr1940 0:cd5ad8e47eb8 72 const float Kp_freq=PI/blend_thresh;
perr1940 0:cd5ad8e47eb8 73 const float Kp_offset=(Kp0+Kp)/2;
perr1940 0:cd5ad8e47eb8 74
perr1940 0:cd5ad8e47eb8 75 float TimeSinceLastStep;
perr1940 0:cd5ad8e47eb8 76 //encoder setup
perr1940 0:cd5ad8e47eb8 77 //float stand_adjust= 10; //positive is more upright.Default 10
perr1940 0:cd5ad8e47eb8 78 //const float zero_ang_L=220+10;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 79 //const float zero_ang_R=40+10;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 80
perr1940 0:cd5ad8e47eb8 81 /** VERIFY THESE VALUES BEFORE RUNNING CODE ON EXO
perr1940 0:cd5ad8e47eb8 82 * USING ERRONEOUS OFFSETS MAY CAUSE PERMANENT DAMAGE!!! */
perr1940 0:cd5ad8e47eb8 83 //const float zero_ang_L=223.83;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 84 //const float zero_ang_R=-176.03;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 85 //const float zero_ang_L=168.24;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 86 //const float zero_ang_R=-128.12;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 87
perr1940 0:cd5ad8e47eb8 88 //const float _enc_high=120; // Hard stop max accepable angle for encoder
perr1940 0:cd5ad8e47eb8 89 //const float _enc_low=-40; // Hard stop min accepable angle for encoder
perr1940 0:cd5ad8e47eb8 90
perr1940 0:cd5ad8e47eb8 91 //PARAMETER ADJUSTMENT - Standing Angle
perr1940 0:cd5ad8e47eb8 92 //Steve = 10
perr1940 0:cd5ad8e47eb8 93 float stand_adjust=10; //positive is more upright//Steve=10 deg
perr1940 0:cd5ad8e47eb8 94 //Person 1
perr1940 0:cd5ad8e47eb8 95 //const float stand_adjust=10;
perr1940 0:cd5ad8e47eb8 96 //Person 2
perr1940 0:cd5ad8e47eb8 97 //const float stand_adjust=10;
perr1940 0:cd5ad8e47eb8 98
perr1940 0:cd5ad8e47eb8 99 //zero_enc_offset_X is the offset for a particular encoder. Must be tuned for an individual encoder based on assembly.
perr1940 0:cd5ad8e47eb8 100 // Offsets for Exo #1. These are related to individual encoder and should never be changed.
perr1940 1:72e0bd01c8e0 101 const float zero_enc_offset_L = 223.83;
perr1940 0:cd5ad8e47eb8 102 // zero_enc_offset_R should be negative
perr1940 1:72e0bd01c8e0 103 const float zero_enc_offset_R = -176.4;
perr1940 0:cd5ad8e47eb8 104 // Offsets for Exo #2
perr1940 0:cd5ad8e47eb8 105 //const float zero_enc_offset_L = 168.24;
perr1940 0:cd5ad8e47eb8 106 // zero_enc_offset_R should be negative
perr1940 0:cd5ad8e47eb8 107 //const float zero_enc_offset_R = -128.12;
perr1940 0:cd5ad8e47eb8 108 //Offset for Test Rig
perr1940 1:72e0bd01c8e0 109 //const float zero_enc_offset_L = 205.24;
perr1940 0:cd5ad8e47eb8 110 // zero_enc_offset_R should be negative
perr1940 1:72e0bd01c8e0 111 //const float zero_enc_offset_R = -135.17;
perr1940 0:cd5ad8e47eb8 112
perr1940 0:cd5ad8e47eb8 113 //zero_ang_X is the zero angle based on the mechanical assembly of the hip and the standing angle parameters of the individual user.
perr1940 0:cd5ad8e47eb8 114 const float zero_ang_L=zero_enc_offset_L+stand_adjust;
perr1940 0:cd5ad8e47eb8 115 const float zero_ang_R=zero_enc_offset_R+stand_adjust;
perr1940 0:cd5ad8e47eb8 116
perr1940 0:cd5ad8e47eb8 117 // Upper and lower limit on Encoder
perr1940 0:cd5ad8e47eb8 118 // TODO(Cashdollar): These may need to be tuned. Window is 160 deg but might not start at 120.
perr1940 0:cd5ad8e47eb8 119 const float _enc_high=122+stand_adjust; // Hard stop max accepable angle for encoder
perr1940 0:cd5ad8e47eb8 120 const float _enc_low=-38+stand_adjust; // Hard stop min accepable angle for encoder
perr1940 0:cd5ad8e47eb8 121
perr1940 0:cd5ad8e47eb8 122
perr1940 0:cd5ad8e47eb8 123 MMgait_t mm_gait_params;
perr1940 0:cd5ad8e47eb8 124 //MMSwing swing(mm_gait_params);
perr1940 0:cd5ad8e47eb8 125 //MMStance stance(mm_gait_params);
perr1940 0:cd5ad8e47eb8 126 MMSwing swing;
perr1940 0:cd5ad8e47eb8 127 MMStance stance;
perr1940 1:72e0bd01c8e0 128 MMFSSwing fsSwing;
perr1940 1:72e0bd01c8e0 129 MMFSStance fsStance;
perr1940 1:72e0bd01c8e0 130 MMFTG ftgSwing;
perr1940 1:72e0bd01c8e0 131 MMFTG ftgStance;
perr1940 1:72e0bd01c8e0 132 LinearBlend blend_left;
perr1940 1:72e0bd01c8e0 133 LinearBlend blend_right;
perr1940 0:cd5ad8e47eb8 134
perr1940 0:cd5ad8e47eb8 135 void initializeExoIOs()
perr1940 0:cd5ad8e47eb8 136 {
perr1940 0:cd5ad8e47eb8 137 // Putting various comm pins high
perr1940 0:cd5ad8e47eb8 138 DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)
perr1940 0:cd5ad8e47eb8 139 pc.baud(921600); //pc.baud(115200);//460800
perr1940 0:cd5ad8e47eb8 140 //pc.baud(230400);
perr1940 0:cd5ad8e47eb8 141
perr1940 0:cd5ad8e47eb8 142 // initialize both knees
perr1940 0:cd5ad8e47eb8 143 knee_L.init();
perr1940 0:cd5ad8e47eb8 144 knee_R.init();
perr1940 0:cd5ad8e47eb8 145 //Setting pwm frequencies to 20kHz
perr1940 0:cd5ad8e47eb8 146
perr1940 0:cd5ad8e47eb8 147 hip_L.off();
perr1940 0:cd5ad8e47eb8 148 hip_R.off();
perr1940 0:cd5ad8e47eb8 149 hip_R.flip();
perr1940 0:cd5ad8e47eb8 150 encoder_L.init(zero_ang_L);
perr1940 0:cd5ad8e47eb8 151 encoder_R.init(zero_ang_R);
perr1940 0:cd5ad8e47eb8 152 encoder_R.flip();
perr1940 0:cd5ad8e47eb8 153 // display debugging info for both knees
perr1940 0:cd5ad8e47eb8 154 //knee_L.debug();
perr1940 0:cd5ad8e47eb8 155 //knee_R.debug();
perr1940 0:cd5ad8e47eb8 156 encoder_L.ranges(_enc_low, _enc_high);
perr1940 0:cd5ad8e47eb8 157 encoder_R.ranges(_enc_low, _enc_high);
perr1940 0:cd5ad8e47eb8 158 encoder_L.rangeCheck();
perr1940 0:cd5ad8e47eb8 159 encoder_R.rangeCheck();
perr1940 0:cd5ad8e47eb8 160 fsm.init();
perr1940 0:cd5ad8e47eb8 161 pc.printf("Test2 \r\n");
perr1940 0:cd5ad8e47eb8 162
perr1940 0:cd5ad8e47eb8 163
perr1940 0:cd5ad8e47eb8 164 dataBedSPI.frequency(10000000);
perr1940 0:cd5ad8e47eb8 165
perr1940 0:cd5ad8e47eb8 166
perr1940 0:cd5ad8e47eb8 167 mm_gait_params.time_steps=900;
perr1940 0:cd5ad8e47eb8 168 mm_gait_params.peak_time=416;
perr1940 0:cd5ad8e47eb8 169 mm_gait_params.stance_start=10;
perr1940 0:cd5ad8e47eb8 170 mm_gait_params.stance_end=-22;
perr1940 0:cd5ad8e47eb8 171 mm_gait_params.max_angle=30;
perr1940 0:cd5ad8e47eb8 172
perr1940 0:cd5ad8e47eb8 173 swing.set(mm_gait_params);
perr1940 0:cd5ad8e47eb8 174 stance.set(mm_gait_params);
perr1940 1:72e0bd01c8e0 175 fsSwing.set(mm_gait_params);
perr1940 1:72e0bd01c8e0 176 fsStance.set(mm_gait_params);
perr1940 1:72e0bd01c8e0 177 ftgSwing.set(mm_gait_params);
perr1940 1:72e0bd01c8e0 178 ftgStance.set(mm_gait_params);
perr1940 0:cd5ad8e47eb8 179 //pc.printf("%Mem: %d \r\n", AvailableMemory());
perr1940 0:cd5ad8e47eb8 180
perr1940 0:cd5ad8e47eb8 181
perr1940 0:cd5ad8e47eb8 182 // print each knee status
perr1940 0:cd5ad8e47eb8 183
perr1940 0:cd5ad8e47eb8 184 pc.printf("knee_L: %x, knee_R:%x\r\n,", knee_L.status(), knee_R.status());
perr1940 0:cd5ad8e47eb8 185
perr1940 0:cd5ad8e47eb8 186
perr1940 0:cd5ad8e47eb8 187 }