Nathaniel Honka / initExoVars

Corrected header file include guards.

Fork of initExoVars by HEL's Angels

initExoVars.cpp@8:ae167702ebcc, 2015-06-28 (annotated)

Committer:
perr1940
Date:
Sun Jun 28 02:09:49 2015 +0000
Revision:
8:ae167702ebcc
Parent:
7:bbcbecee4183
Child:
9:6cf157a6d58b
don't know what I changed

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 5:fcd299262d0a 58 float Kp_Swing = .02;
perr1940 5:fcd299262d0a 59 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 8:ae167702ebcc 65 float Kd_Swing=0.002;
perr1940 8:ae167702ebcc 66 float Kd_Stance=0.002;
perr1940 0:cd5ad8e47eb8 67
perr1940 0:cd5ad8e47eb8 68 const float Kp=.04;
perr1940 0:cd5ad8e47eb8 69 const float Kp0=.001;
perr1940 5:fcd299262d0a 70 float Kd=.002;
perr1940 0:cd5ad8e47eb8 71 const float sat=.2;
perr1940 0:cd5ad8e47eb8 72 const int blend_thresh=250; //samples before we switch to the true trajectory
perr1940 0:cd5ad8e47eb8 73 const float Kp_mag=(Kp0-Kp)/2;
perr1940 0:cd5ad8e47eb8 74 const float Kp_freq=PI/blend_thresh;
perr1940 0:cd5ad8e47eb8 75 const float Kp_offset=(Kp0+Kp)/2;
perr1940 0:cd5ad8e47eb8 76
perr1940 0:cd5ad8e47eb8 77 float TimeSinceLastStep;
perr1940 0:cd5ad8e47eb8 78 //encoder setup
perr1940 0:cd5ad8e47eb8 79 //float stand_adjust= 10; //positive is more upright.Default 10
perr1940 0:cd5ad8e47eb8 80 //const float zero_ang_L=220+10;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 81 //const float zero_ang_R=40+10;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 82
perr1940 0:cd5ad8e47eb8 83 /** VERIFY THESE VALUES BEFORE RUNNING CODE ON EXO
perr1940 0:cd5ad8e47eb8 84 * USING ERRONEOUS OFFSETS MAY CAUSE PERMANENT DAMAGE!!! */
perr1940 0:cd5ad8e47eb8 85 //const float zero_ang_L=223.83;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 86 //const float zero_ang_R=-176.03;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 87 //const float zero_ang_L=168.24;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 88 //const float zero_ang_R=-128.12;//approximate point where the pilot should be standing straight
perr1940 0:cd5ad8e47eb8 89
perr1940 0:cd5ad8e47eb8 90 //const float _enc_high=120; // Hard stop max accepable angle for encoder
perr1940 0:cd5ad8e47eb8 91 //const float _enc_low=-40; // Hard stop min accepable angle for encoder
perr1940 0:cd5ad8e47eb8 92
perr1940 0:cd5ad8e47eb8 93 //PARAMETER ADJUSTMENT - Standing Angle
perr1940 0:cd5ad8e47eb8 94 //Steve = 10
cashdollar 2:650a015179b9 95 float stand_adjust=0; //positive is more upright//Steve=10 deg
perr1940 0:cd5ad8e47eb8 96 //Person 1
perr1940 0:cd5ad8e47eb8 97 //const float stand_adjust=10;
perr1940 0:cd5ad8e47eb8 98 //Person 2
perr1940 0:cd5ad8e47eb8 99 //const float stand_adjust=10;
cashdollar 2:650a015179b9 100
perr1940 0:cd5ad8e47eb8 101 //zero_enc_offset_X is the offset for a particular encoder. Must be tuned for an individual encoder based on assembly.
perr1940 0:cd5ad8e47eb8 102 // Offsets for Exo #1. These are related to individual encoder and should never be changed.
perr1940 8:ae167702ebcc 103 float zero_enc_offset_L = 223.83;
perr1940 0:cd5ad8e47eb8 104 // zero_enc_offset_R should be negative
perr1940 8:ae167702ebcc 105 float zero_enc_offset_R = -176.4;
perr1940 8:ae167702ebcc 106 float bob_dole=-176.4;
perr1940 5:fcd299262d0a 107 // Offsets for Test Rig
perr1940 8:ae167702ebcc 108 //float zero_enc_offset_L = 205.24;
perr1940 0:cd5ad8e47eb8 109 // zero_enc_offset_R should be negative
perr1940 8:ae167702ebcc 110 //float zero_enc_offset_R = -135.17;
perr1940 8:ae167702ebcc 111 //float bob_dole=-135.17;
cashdollar 2:650a015179b9 112
cashdollar 2:650a015179b9 113
perr1940 0:cd5ad8e47eb8 114 //Offset for Test Rig
cashdollar 2:650a015179b9 115 //const float zero_enc_offset_L = 205.24;
perr1940 0:cd5ad8e47eb8 116 // zero_enc_offset_R should be negative
cashdollar 2:650a015179b9 117 //const float zero_enc_offset_R = -135.17;
cashdollar 2:650a015179b9 118
cashdollar 2:650a015179b9 119
perr1940 0:cd5ad8e47eb8 120 //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 121 //const float zero_ang_L=zero_enc_offset_L+stand_adjust;
cashdollar 2:650a015179b9 122 //const float zero_ang_R=zero_enc_offset_R+stand_adjust;
cashdollar 2:650a015179b9 123 float zero_ang_L;
cashdollar 2:650a015179b9 124 float zero_ang_R;
cashdollar 2:650a015179b9 125
perr1940 0:cd5ad8e47eb8 126 // Upper and lower limit on Encoder
perr1940 0:cd5ad8e47eb8 127 // TODO(Cashdollar): These may need to be tuned. Window is 160 deg but might not start at 120.
perr1940 0:cd5ad8e47eb8 128 const float _enc_high=122+stand_adjust; // Hard stop max accepable angle for encoder
perr1940 0:cd5ad8e47eb8 129 const float _enc_low=-38+stand_adjust; // Hard stop min accepable angle for encoder
perr1940 0:cd5ad8e47eb8 130
perr1940 0:cd5ad8e47eb8 131
perr1940 0:cd5ad8e47eb8 132 MMgait_t mm_gait_params;
perr1940 0:cd5ad8e47eb8 133 //MMSwing swing(mm_gait_params);
perr1940 0:cd5ad8e47eb8 134 //MMStance stance(mm_gait_params);
perr1940 0:cd5ad8e47eb8 135 MMSwing swing;
perr1940 0:cd5ad8e47eb8 136 MMStance stance;
cashdollar 2:650a015179b9 137 MMFSSwing fsSwing;
cashdollar 2:650a015179b9 138 MMFSStance fsStance;
cashdollar 2:650a015179b9 139 MMFTG ftgSwing;
cashdollar 2:650a015179b9 140 MMFTG ftgStance;
cashdollar 4:bd8994714003 141 LinearBlend blend_left;
cashdollar 4:bd8994714003 142 LinearBlend blend_right;
cashdollar 2:650a015179b9 143
cashdollar 3:e910d4eb3a36 144 /*void readCalib()
cashdollar 2:650a015179b9 145 {
cashdollar 2:650a015179b9 146 char A[10];
cashdollar 2:650a015179b9 147 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
cashdollar 2:650a015179b9 148 FILE *set = fopen("/local/calib.txt", "r"); // Open "setup.txt" on the local file system for read
cashdollar 2:650a015179b9 149 fscanf(set,"%s %f",A,&zero_enc_offset_L); // read offset
cashdollar 2:650a015179b9 150 fscanf(set,"%s %f",A,&zero_enc_offset_R); // read offset
cashdollar 2:650a015179b9 151 fclose(set);
cashdollar 2:650a015179b9 152 wait(1); //TODO(Cashdollar): reduce this time after testing
cashdollar 3:e910d4eb3a36 153 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);
cashdollar 3:e910d4eb3a36 154 wait(1); //TODO(Cashdollar): Delete after testing, this is to make it easier to see what is happening
cashdollar 3:e910d4eb3a36 155 }*/
perr1940 0:cd5ad8e47eb8 156
perr1940 0:cd5ad8e47eb8 157 void initializeExoIOs()
perr1940 0:cd5ad8e47eb8 158 {
cashdollar 4:bd8994714003 159 pc.printf("\r\n entered initexoios");
perr1940 0:cd5ad8e47eb8 160 // Putting various comm pins high
perr1940 0:cd5ad8e47eb8 161 DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)
cashdollar 4:bd8994714003 162 //pc.baud(921600);
cashdollar 3:e910d4eb3a36 163 //pc.baud(921600);
cashdollar 3:e910d4eb3a36 164 //readCalib();
cashdollar 2:650a015179b9 165 zero_ang_L=zero_enc_offset_L+stand_adjust;
cashdollar 2:650a015179b9 166 zero_ang_R=zero_enc_offset_R+stand_adjust;
cashdollar 4:bd8994714003 167 pc.printf("\r\nzero ang assigned");
perr1940 0:cd5ad8e47eb8 168
perr1940 0:cd5ad8e47eb8 169 // initialize both knees
perr1940 0:cd5ad8e47eb8 170 knee_L.init();
perr1940 0:cd5ad8e47eb8 171 knee_R.init();
cashdollar 3:e910d4eb3a36 172 //Setting pwm frequencies to 20kHz
cashdollar 4:bd8994714003 173 pc.printf("\r\n knees init");
perr1940 0:cd5ad8e47eb8 174
perr1940 0:cd5ad8e47eb8 175 hip_L.off();
perr1940 0:cd5ad8e47eb8 176 hip_R.off();
perr1940 0:cd5ad8e47eb8 177 hip_R.flip();
perr1940 0:cd5ad8e47eb8 178 encoder_L.init(zero_ang_L);
perr1940 0:cd5ad8e47eb8 179 encoder_R.init(zero_ang_R);
perr1940 0:cd5ad8e47eb8 180 encoder_R.flip();
perr1940 0:cd5ad8e47eb8 181 // display debugging info for both knees
perr1940 0:cd5ad8e47eb8 182 //knee_L.debug();
perr1940 0:cd5ad8e47eb8 183 //knee_R.debug();
perr1940 0:cd5ad8e47eb8 184 encoder_L.ranges(_enc_low, _enc_high);
perr1940 0:cd5ad8e47eb8 185 encoder_R.ranges(_enc_low, _enc_high);
perr1940 0:cd5ad8e47eb8 186 encoder_L.rangeCheck();
perr1940 0:cd5ad8e47eb8 187 encoder_R.rangeCheck();
perr1940 0:cd5ad8e47eb8 188 fsm.init();
perr1940 0:cd5ad8e47eb8 189 pc.printf("Test2 \r\n");
perr1940 0:cd5ad8e47eb8 190
perr1940 0:cd5ad8e47eb8 191
perr1940 0:cd5ad8e47eb8 192 dataBedSPI.frequency(10000000);
perr1940 0:cd5ad8e47eb8 193
perr1940 0:cd5ad8e47eb8 194
perr1940 0:cd5ad8e47eb8 195 mm_gait_params.time_steps=900;
perr1940 0:cd5ad8e47eb8 196 mm_gait_params.peak_time=416;
perr1940 0:cd5ad8e47eb8 197 mm_gait_params.stance_start=10;
perr1940 0:cd5ad8e47eb8 198 mm_gait_params.stance_end=-22;
perr1940 0:cd5ad8e47eb8 199 mm_gait_params.max_angle=30;
cashdollar 4:bd8994714003 200 mm_gait_params.max_fs_angle=mm_gait_params.max_angle*1.16;
perr1940 0:cd5ad8e47eb8 201
perr1940 0:cd5ad8e47eb8 202 swing.set(mm_gait_params);
perr1940 0:cd5ad8e47eb8 203 stance.set(mm_gait_params);
cashdollar 4:bd8994714003 204 fsSwing.set(mm_gait_params);
cashdollar 4:bd8994714003 205 fsStance.set(mm_gait_params);
cashdollar 4:bd8994714003 206 ftgSwing.set(mm_gait_params);
cashdollar 4:bd8994714003 207 ftgStance.set(mm_gait_params);
perr1940 0:cd5ad8e47eb8 208 //pc.printf("%Mem: %d \r\n", AvailableMemory());
perr1940 0:cd5ad8e47eb8 209
perr1940 0:cd5ad8e47eb8 210
perr1940 0:cd5ad8e47eb8 211 // print each knee status
perr1940 0:cd5ad8e47eb8 212
perr1940 0:cd5ad8e47eb8 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
perr1940 0:cd5ad8e47eb8 216 }
cashdollar 2:650a015179b9 217