Nathaniel Honka / initExoVars

Corrected header file include guards.

Fork of initExoVars by HEL's Angels

initExoVars.cpp@13:268f4564a809, 2015-07-02 (annotated)

Committer:
nathanhonka
Date:
Thu Jul 02 21:38:21 2015 +0000
Revision:
13:268f4564a809
Parent:
12:3a5a9bbf1c93 
Corrected header file include guards (did not encapsulate entire header).

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