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@6:87ad618621a4, 2015-06-26 (annotated)

Committer:
perr1940
Date:
Fri Jun 26 22:48:28 2015 +0000
Revision:
6:87ad618621a4
Parent:
5:fcd299262d0a
Child:
7:bbcbecee4183
small stupid change.  zero_enc_offset_R doesn't work

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 0:cd5ad8e47eb8 65
perr1940 0:cd5ad8e47eb8 66 const float Kp=.04;
perr1940 0:cd5ad8e47eb8 67 const float Kp0=.001;
perr1940 5:fcd299262d0a 68 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
cashdollar 2:650a015179b9 93 float stand_adjust=0; //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;
cashdollar 2:650a015179b9 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 6:87ad618621a4 101 float zero_enc_offset_L = 223.83;
perr1940 0:cd5ad8e47eb8 102 // zero_enc_offset_R should be negative
perr1940 5:fcd299262d0a 103 float zero_enc_offset_R = -176.4;
perr1940 6:87ad618621a4 104 float bob_dole=-176.4;
perr1940 5:fcd299262d0a 105 // Offsets for Test Rig
perr1940 5:fcd299262d0a 106 //float zero_enc_offset_L = 205.24;
perr1940 0:cd5ad8e47eb8 107 // zero_enc_offset_R should be negative
perr1940 5:fcd299262d0a 108 //float zero_enc_offset_R = -135.17;
cashdollar 2:650a015179b9 109
cashdollar 2:650a015179b9 110
perr1940 0:cd5ad8e47eb8 111 //Offset for Test Rig
cashdollar 2:650a015179b9 112 //const float zero_enc_offset_L = 205.24;
perr1940 0:cd5ad8e47eb8 113 // zero_enc_offset_R should be negative
cashdollar 2:650a015179b9 114 //const float zero_enc_offset_R = -135.17;
cashdollar 2:650a015179b9 115
cashdollar 2:650a015179b9 116
perr1940 0:cd5ad8e47eb8 117 //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 118 //const float zero_ang_L=zero_enc_offset_L+stand_adjust;
cashdollar 2:650a015179b9 119 //const float zero_ang_R=zero_enc_offset_R+stand_adjust;
cashdollar 2:650a015179b9 120 float zero_ang_L;
cashdollar 2:650a015179b9 121 float zero_ang_R;
cashdollar 2:650a015179b9 122
perr1940 0:cd5ad8e47eb8 123 // Upper and lower limit on Encoder
perr1940 0:cd5ad8e47eb8 124 // TODO(Cashdollar): These may need to be tuned. Window is 160 deg but might not start at 120.
perr1940 0:cd5ad8e47eb8 125 const float _enc_high=122+stand_adjust; // Hard stop max accepable angle for encoder
perr1940 0:cd5ad8e47eb8 126 const float _enc_low=-38+stand_adjust; // Hard stop min accepable angle for encoder
perr1940 0:cd5ad8e47eb8 127
perr1940 0:cd5ad8e47eb8 128
perr1940 0:cd5ad8e47eb8 129 MMgait_t mm_gait_params;
perr1940 0:cd5ad8e47eb8 130 //MMSwing swing(mm_gait_params);
perr1940 0:cd5ad8e47eb8 131 //MMStance stance(mm_gait_params);
perr1940 0:cd5ad8e47eb8 132 MMSwing swing;
perr1940 0:cd5ad8e47eb8 133 MMStance stance;
cashdollar 2:650a015179b9 134 MMFSSwing fsSwing;
cashdollar 2:650a015179b9 135 MMFSStance fsStance;
cashdollar 2:650a015179b9 136 MMFTG ftgSwing;
cashdollar 2:650a015179b9 137 MMFTG ftgStance;
cashdollar 4:bd8994714003 138 LinearBlend blend_left;
cashdollar 4:bd8994714003 139 LinearBlend blend_right;
cashdollar 2:650a015179b9 140
cashdollar 3:e910d4eb3a36 141 /*void readCalib()
cashdollar 2:650a015179b9 142 {
cashdollar 2:650a015179b9 143 char A[10];
cashdollar 2:650a015179b9 144 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
cashdollar 2:650a015179b9 145 FILE *set = fopen("/local/calib.txt", "r"); // Open "setup.txt" on the local file system for read
cashdollar 2:650a015179b9 146 fscanf(set,"%s %f",A,&zero_enc_offset_L); // read offset
cashdollar 2:650a015179b9 147 fscanf(set,"%s %f",A,&zero_enc_offset_R); // read offset
cashdollar 2:650a015179b9 148 fclose(set);
cashdollar 2:650a015179b9 149 wait(1); //TODO(Cashdollar): reduce this time after testing
cashdollar 3:e910d4eb3a36 150 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 151 wait(1); //TODO(Cashdollar): Delete after testing, this is to make it easier to see what is happening
cashdollar 3:e910d4eb3a36 152 }*/
perr1940 0:cd5ad8e47eb8 153
perr1940 0:cd5ad8e47eb8 154 void initializeExoIOs()
perr1940 0:cd5ad8e47eb8 155 {
cashdollar 4:bd8994714003 156 pc.printf("\r\n entered initexoios");
perr1940 0:cd5ad8e47eb8 157 // Putting various comm pins high
perr1940 0:cd5ad8e47eb8 158 DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)
cashdollar 4:bd8994714003 159 //pc.baud(921600);
cashdollar 3:e910d4eb3a36 160 //pc.baud(921600);
cashdollar 3:e910d4eb3a36 161 //readCalib();
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;
cashdollar 4:bd8994714003 164 pc.printf("\r\nzero ang assigned");
perr1940 0:cd5ad8e47eb8 165
perr1940 0:cd5ad8e47eb8 166 // initialize both knees
perr1940 0:cd5ad8e47eb8 167 knee_L.init();
perr1940 0:cd5ad8e47eb8 168 knee_R.init();
cashdollar 3:e910d4eb3a36 169 //Setting pwm frequencies to 20kHz
cashdollar 4:bd8994714003 170 pc.printf("\r\n knees init");
perr1940 0:cd5ad8e47eb8 171
perr1940 0:cd5ad8e47eb8 172 hip_L.off();
perr1940 0:cd5ad8e47eb8 173 hip_R.off();
perr1940 0:cd5ad8e47eb8 174 hip_R.flip();
perr1940 0:cd5ad8e47eb8 175 encoder_L.init(zero_ang_L);
perr1940 0:cd5ad8e47eb8 176 encoder_R.init(zero_ang_R);
perr1940 0:cd5ad8e47eb8 177 encoder_R.flip();
perr1940 0:cd5ad8e47eb8 178 // display debugging info for both knees
perr1940 0:cd5ad8e47eb8 179 //knee_L.debug();
perr1940 0:cd5ad8e47eb8 180 //knee_R.debug();
perr1940 0:cd5ad8e47eb8 181 encoder_L.ranges(_enc_low, _enc_high);
perr1940 0:cd5ad8e47eb8 182 encoder_R.ranges(_enc_low, _enc_high);
perr1940 0:cd5ad8e47eb8 183 encoder_L.rangeCheck();
perr1940 0:cd5ad8e47eb8 184 encoder_R.rangeCheck();
perr1940 0:cd5ad8e47eb8 185 fsm.init();
perr1940 0:cd5ad8e47eb8 186 pc.printf("Test2 \r\n");
perr1940 0:cd5ad8e47eb8 187
perr1940 0:cd5ad8e47eb8 188
perr1940 0:cd5ad8e47eb8 189 dataBedSPI.frequency(10000000);
perr1940 0:cd5ad8e47eb8 190
perr1940 0:cd5ad8e47eb8 191
perr1940 0:cd5ad8e47eb8 192 mm_gait_params.time_steps=900;
perr1940 0:cd5ad8e47eb8 193 mm_gait_params.peak_time=416;
perr1940 0:cd5ad8e47eb8 194 mm_gait_params.stance_start=10;
perr1940 0:cd5ad8e47eb8 195 mm_gait_params.stance_end=-22;
perr1940 0:cd5ad8e47eb8 196 mm_gait_params.max_angle=30;
cashdollar 4:bd8994714003 197 mm_gait_params.max_fs_angle=mm_gait_params.max_angle*1.16;
perr1940 0:cd5ad8e47eb8 198
perr1940 0:cd5ad8e47eb8 199 swing.set(mm_gait_params);
perr1940 0:cd5ad8e47eb8 200 stance.set(mm_gait_params);
cashdollar 4:bd8994714003 201 fsSwing.set(mm_gait_params);
cashdollar 4:bd8994714003 202 fsStance.set(mm_gait_params);
cashdollar 4:bd8994714003 203 ftgSwing.set(mm_gait_params);
cashdollar 4:bd8994714003 204 ftgStance.set(mm_gait_params);
perr1940 0:cd5ad8e47eb8 205 //pc.printf("%Mem: %d \r\n", AvailableMemory());
perr1940 0:cd5ad8e47eb8 206
perr1940 0:cd5ad8e47eb8 207
perr1940 0:cd5ad8e47eb8 208 // print each knee status
perr1940 0:cd5ad8e47eb8 209
perr1940 0:cd5ad8e47eb8 210 pc.printf("knee_L: %x, knee_R:%x\r\n,", knee_L.status(), knee_R.status());
perr1940 0:cd5ad8e47eb8 211
perr1940 0:cd5ad8e47eb8 212
perr1940 0:cd5ad8e47eb8 213 }
cashdollar 2:650a015179b9 214