Nathaniel Honka / initExoVars

Corrected header file include guards.

Fork of initExoVars by HEL's Angels

initExoVars.cpp@9:6cf157a6d58b, 2015-06-29 (annotated)

Committer:
perr1940
Date:
Mon Jun 29 21:20:35 2015 +0000
Revision:
9:6cf157a6d58b
Parent:
8:ae167702ebcc
Child:
10:29955bf36492
changed stand_adjust to start at 10

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
perr1940 9:6cf157a6d58b 95 float stand_adjust=10; //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 9:6cf157a6d58b 103 //float zero_enc_offset_L = 223.83;
perr1940 0:cd5ad8e47eb8 104 // zero_enc_offset_R should be negative
perr1940 9:6cf157a6d58b 105 //float zero_enc_offset_R = -176.4;
perr1940 9:6cf157a6d58b 106 //float bob_dole=-176.4;
perr1940 5:fcd299262d0a 107 // Offsets for Test Rig
perr1940 9:6cf157a6d58b 108 float zero_enc_offset_L = 205.24;
perr1940 0:cd5ad8e47eb8 109 // zero_enc_offset_R should be negative
perr1940 9:6cf157a6d58b 110 float zero_enc_offset_R = -135.17;
perr1940 9:6cf157a6d58b 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 9:6cf157a6d58b 133 Brad_poly_gait_t gait_params;
perr1940 0:cd5ad8e47eb8 134 //MMSwing swing(mm_gait_params);
perr1940 0:cd5ad8e47eb8 135 //MMStance stance(mm_gait_params);
perr1940 9:6cf157a6d58b 136 //MMSwing swing;
perr1940 9:6cf157a6d58b 137 //MMStance stance;
perr1940 9:6cf157a6d58b 138 BradPolySwing swing;
perr1940 9:6cf157a6d58b 139 BradPolyStance stance;
cashdollar 2:650a015179b9 140 MMFSSwing fsSwing;
cashdollar 2:650a015179b9 141 MMFSStance fsStance;
cashdollar 2:650a015179b9 142 MMFTG ftgSwing;
cashdollar 2:650a015179b9 143 MMFTG ftgStance;
cashdollar 4:bd8994714003 144 LinearBlend blend_left;
cashdollar 4:bd8994714003 145 LinearBlend blend_right;
cashdollar 2:650a015179b9 146
cashdollar 3:e910d4eb3a36 147 /*void readCalib()
cashdollar 2:650a015179b9 148 {
cashdollar 2:650a015179b9 149 char A[10];
cashdollar 2:650a015179b9 150 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
cashdollar 2:650a015179b9 151 FILE *set = fopen("/local/calib.txt", "r"); // Open "setup.txt" on the local file system for read
cashdollar 2:650a015179b9 152 fscanf(set,"%s %f",A,&zero_enc_offset_L); // read offset
cashdollar 2:650a015179b9 153 fscanf(set,"%s %f",A,&zero_enc_offset_R); // read offset
cashdollar 2:650a015179b9 154 fclose(set);
cashdollar 2:650a015179b9 155 wait(1); //TODO(Cashdollar): reduce this time after testing
cashdollar 3:e910d4eb3a36 156 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 157 wait(1); //TODO(Cashdollar): Delete after testing, this is to make it easier to see what is happening
cashdollar 3:e910d4eb3a36 158 }*/
perr1940 0:cd5ad8e47eb8 159
perr1940 0:cd5ad8e47eb8 160 void initializeExoIOs()
perr1940 0:cd5ad8e47eb8 161 {
cashdollar 4:bd8994714003 162 pc.printf("\r\n entered initexoios");
perr1940 0:cd5ad8e47eb8 163 // Putting various comm pins high
perr1940 0:cd5ad8e47eb8 164 DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)
cashdollar 4:bd8994714003 165 //pc.baud(921600);
cashdollar 3:e910d4eb3a36 166 //pc.baud(921600);
cashdollar 3:e910d4eb3a36 167 //readCalib();
cashdollar 2:650a015179b9 168 zero_ang_L=zero_enc_offset_L+stand_adjust;
cashdollar 2:650a015179b9 169 zero_ang_R=zero_enc_offset_R+stand_adjust;
cashdollar 4:bd8994714003 170 pc.printf("\r\nzero ang assigned");
perr1940 0:cd5ad8e47eb8 171
perr1940 0:cd5ad8e47eb8 172 // initialize both knees
perr1940 0:cd5ad8e47eb8 173 knee_L.init();
perr1940 0:cd5ad8e47eb8 174 knee_R.init();
cashdollar 3:e910d4eb3a36 175 //Setting pwm frequencies to 20kHz
cashdollar 4:bd8994714003 176 pc.printf("\r\n knees init");
perr1940 0:cd5ad8e47eb8 177
perr1940 0:cd5ad8e47eb8 178 hip_L.off();
perr1940 0:cd5ad8e47eb8 179 hip_R.off();
perr1940 0:cd5ad8e47eb8 180 hip_R.flip();
perr1940 0:cd5ad8e47eb8 181 encoder_L.init(zero_ang_L);
perr1940 0:cd5ad8e47eb8 182 encoder_R.init(zero_ang_R);
perr1940 0:cd5ad8e47eb8 183 encoder_R.flip();
perr1940 0:cd5ad8e47eb8 184 // display debugging info for both knees
perr1940 0:cd5ad8e47eb8 185 //knee_L.debug();
perr1940 0:cd5ad8e47eb8 186 //knee_R.debug();
perr1940 0:cd5ad8e47eb8 187 encoder_L.ranges(_enc_low, _enc_high);
perr1940 0:cd5ad8e47eb8 188 encoder_R.ranges(_enc_low, _enc_high);
perr1940 0:cd5ad8e47eb8 189 encoder_L.rangeCheck();
perr1940 0:cd5ad8e47eb8 190 encoder_R.rangeCheck();
perr1940 0:cd5ad8e47eb8 191 fsm.init();
perr1940 0:cd5ad8e47eb8 192 pc.printf("Test2 \r\n");
perr1940 0:cd5ad8e47eb8 193
perr1940 0:cd5ad8e47eb8 194
perr1940 0:cd5ad8e47eb8 195 dataBedSPI.frequency(10000000);
perr1940 0:cd5ad8e47eb8 196
perr1940 0:cd5ad8e47eb8 197
perr1940 0:cd5ad8e47eb8 198 mm_gait_params.time_steps=900;
perr1940 0:cd5ad8e47eb8 199 mm_gait_params.peak_time=416;
perr1940 0:cd5ad8e47eb8 200 mm_gait_params.stance_start=10;
perr1940 0:cd5ad8e47eb8 201 mm_gait_params.stance_end=-22;
perr1940 0:cd5ad8e47eb8 202 mm_gait_params.max_angle=30;
cashdollar 4:bd8994714003 203 mm_gait_params.max_fs_angle=mm_gait_params.max_angle*1.16;
perr1940 0:cd5ad8e47eb8 204
perr1940 9:6cf157a6d58b 205 gait_params.time_steps=900;
perr1940 9:6cf157a6d58b 206 gait_params.peak_time=416;
perr1940 9:6cf157a6d58b 207 gait_params.stance_start=10;
perr1940 9:6cf157a6d58b 208 gait_params.stance_end=-22;
perr1940 9:6cf157a6d58b 209 gait_params.max_angle=30;
perr1940 9:6cf157a6d58b 210 gait_params.max_fs_angle=mm_gait_params.max_angle*1.16;
perr1940 9:6cf157a6d58b 211
perr1940 9:6cf157a6d58b 212 //swing.set(mm_gait_params);
perr1940 9:6cf157a6d58b 213 //stance.set(mm_gait_params);
perr1940 9:6cf157a6d58b 214 swing.set(gait_params);
perr1940 9:6cf157a6d58b 215 stance.set(gait_params);
cashdollar 4:bd8994714003 216 fsSwing.set(mm_gait_params);
cashdollar 4:bd8994714003 217 fsStance.set(mm_gait_params);
cashdollar 4:bd8994714003 218 ftgSwing.set(mm_gait_params);
cashdollar 4:bd8994714003 219 ftgStance.set(mm_gait_params);
perr1940 0:cd5ad8e47eb8 220 //pc.printf("%Mem: %d \r\n", AvailableMemory());
perr1940 0:cd5ad8e47eb8 221
perr1940 0:cd5ad8e47eb8 222
perr1940 0:cd5ad8e47eb8 223 // print each knee status
perr1940 0:cd5ad8e47eb8 224
perr1940 0:cd5ad8e47eb8 225 pc.printf("knee_L: %x, knee_R:%x\r\n,", knee_L.status(), knee_R.status());
perr1940 0:cd5ad8e47eb8 226
perr1940 0:cd5ad8e47eb8 227
perr1940 0:cd5ad8e47eb8 228 }
cashdollar 2:650a015179b9 229