Suraj Giri / Mbed 2 deprecated heros_leg_readout_torque_addition_V3

Version 3 is with update to the test rig with a linear actuator

Dependencies:   SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect LCM101 LinearActuator

main.cpp@4:1cdce6c6c94e, 2018-10-12 (annotated)

Committer:
cnckiwi31
Date:
Fri Oct 12 12:12:55 2018 +0000
Revision:
4:1cdce6c6c94e
Parent:
2:84d479fe9b5e
Child:
5:63063a9fa51c
HeRoS: read out and log joint angles and force sensor data from the leg test bench.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:3855d4588f76 1 #include "mbed.h"
cnckiwi31 4:1cdce6c6c94e 2 #include <string>
megrootens 0:3855d4588f76 3
megrootens 0:3855d4588f76 4 #include "bench.h"
megrootens 0:3855d4588f76 5 #include "PinDetect.h"
megrootens 0:3855d4588f76 6 #include "SDFileSystem.h"
megrootens 0:3855d4588f76 7 #include "MODSERIAL.h"
megrootens 0:3855d4588f76 8
Technical_Muffin 2:84d479fe9b5e 9 #define PI 3.14159265
megrootens 0:3855d4588f76 10
megrootens 0:3855d4588f76 11 // User io
cnckiwi31 4:1cdce6c6c94e 12
cnckiwi31 4:1cdce6c6c94e 13 PinDetect forceNull(PTB16,PullUp);
cnckiwi31 4:1cdce6c6c94e 14 PinDetect press0Null(PTA4,PullUp);
cnckiwi31 4:1cdce6c6c94e 15
megrootens 0:3855d4588f76 16 PinDetect sw2(SW2,PullUp);
megrootens 0:3855d4588f76 17 PinDetect sw3(SW3,PullUp);
megrootens 0:3855d4588f76 18 DigitalOut led_g(LED_GREEN);
megrootens 0:3855d4588f76 19
cnckiwi31 4:1cdce6c6c94e 20 void ResetForce();
cnckiwi31 4:1cdce6c6c94e 21 void ResetPress();
megrootens 0:3855d4588f76 22 void TogglePrinting();
megrootens 0:3855d4588f76 23 void ToggleLogging();
megrootens 0:3855d4588f76 24 void ShowAlive();
megrootens 0:3855d4588f76 25
megrootens 0:3855d4588f76 26 // Bench
cnckiwi31 4:1cdce6c6c94e 27 Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101,SPTE_0,SPTE_1);
megrootens 0:3855d4588f76 28 void Update()
megrootens 0:3855d4588f76 29 {
megrootens 0:3855d4588f76 30 leg.Update();
megrootens 0:3855d4588f76 31 }
megrootens 0:3855d4588f76 32
megrootens 0:3855d4588f76 33 // SD Card
megrootens 0:3855d4588f76 34 SDFileSystem sd(SD_MOSI, SD_MISO, SD_SCK, SD_CS, "sd");
megrootens 0:3855d4588f76 35
megrootens 0:3855d4588f76 36 void InitSdCard();
megrootens 0:3855d4588f76 37 void StartLogging(const char * fname_append = "data");
megrootens 0:3855d4588f76 38 void StopLogging();
megrootens 0:3855d4588f76 39 void LogData();
megrootens 0:3855d4588f76 40
megrootens 0:3855d4588f76 41 // Serial
megrootens 0:3855d4588f76 42 MODSERIAL pc(USBTX,USBRX);
megrootens 0:3855d4588f76 43
megrootens 0:3855d4588f76 44 void PrintStatus();
megrootens 0:3855d4588f76 45 void PrintMenu();
megrootens 0:3855d4588f76 46
megrootens 0:3855d4588f76 47 // Timing
megrootens 0:3855d4588f76 48 Ticker tick_update, tick_serial, tick_logging;
megrootens 0:3855d4588f76 49 Timer timer;
cnckiwi31 4:1cdce6c6c94e 50 int firstReadMS = 0; //first timer value read
cnckiwi31 4:1cdce6c6c94e 51 bool startedLogging = false; //in the middle of a logging cycle
megrootens 0:3855d4588f76 52
megrootens 0:3855d4588f76 53 /**
megrootens 0:3855d4588f76 54 * Main loop/
megrootens 0:3855d4588f76 55 */
megrootens 0:3855d4588f76 56 int main()
megrootens 0:3855d4588f76 57 {
megrootens 0:3855d4588f76 58 pc.baud(timing::kSerialBaudrate);
megrootens 0:3855d4588f76 59 pc.printf("**Hello!**\r\n");
megrootens 0:3855d4588f76 60
megrootens 0:3855d4588f76 61 InitSdCard();
megrootens 0:3855d4588f76 62
megrootens 0:3855d4588f76 63 tick_update.attach_us(&Update,timing::kTimeControlUs);
megrootens 0:3855d4588f76 64 tick_serial.attach_us(&PrintStatus,timing::kTimeSerialPrintUs);
megrootens 0:3855d4588f76 65
megrootens 0:3855d4588f76 66 PrintMenu();
megrootens 0:3855d4588f76 67
cnckiwi31 4:1cdce6c6c94e 68 forceNull.attach_asserted(&ResetForce);
cnckiwi31 4:1cdce6c6c94e 69 press0Null.attach_asserted(&ResetPress);
cnckiwi31 4:1cdce6c6c94e 70
cnckiwi31 4:1cdce6c6c94e 71 forceNull.setSampleFrequency();
cnckiwi31 4:1cdce6c6c94e 72 press0Null.setSampleFrequency();
cnckiwi31 4:1cdce6c6c94e 73
megrootens 0:3855d4588f76 74 sw2.attach_asserted(&TogglePrinting);
megrootens 0:3855d4588f76 75 sw3.attach_asserted(&ToggleLogging);
megrootens 0:3855d4588f76 76
megrootens 0:3855d4588f76 77 sw2.setSampleFrequency();
megrootens 0:3855d4588f76 78 sw3.setSampleFrequency();
megrootens 0:3855d4588f76 79
megrootens 0:3855d4588f76 80 while (true);
megrootens 0:3855d4588f76 81 }
Technical_Muffin 2:84d479fe9b5e 82 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
Technical_Muffin 2:84d479fe9b5e 83 // IMPLEMENTATION torque calculations
Technical_Muffin 2:84d479fe9b5e 84 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
Technical_Muffin 2:84d479fe9b5e 85 float getTorque(float theta_hip, float theta_knee, float M0)
Technical_Muffin 2:84d479fe9b5e 86 {
Technical_Muffin 2:84d479fe9b5e 87 float theta_shin = theta_knee - theta_hip;
Technical_Muffin 2:84d479fe9b5e 88 //Values for torque calculation
Technical_Muffin 2:84d479fe9b5e 89 const float Mt = 7.24;//mass of thigh in kg
Technical_Muffin 2:84d479fe9b5e 90 const float Ms = 3.06;//mass of shank in kg
Technical_Muffin 2:84d479fe9b5e 91 const float Marm = 2.0;//mass of load carrying arm in kg
Technical_Muffin 2:84d479fe9b5e 92 const float g = 9.81;//gravity coefficient in m/s2
Technical_Muffin 2:84d479fe9b5e 93 const float dt = 0.45;//length of thigh in m
Technical_Muffin 2:84d479fe9b5e 94 const float dtm = 0.25755;//distance to mass middle point thigh in m
Technical_Muffin 2:84d479fe9b5e 95 const float ds = 0.45;//length of shank in m
Technical_Muffin 2:84d479fe9b5e 96 const float dsm = 0.197134;//distance to mass middle point shank in m
Technical_Muffin 2:84d479fe9b5e 97 const float shank_angle_correction = 6.04;//corrections for the difference in moment arm angle
Technical_Muffin 2:84d479fe9b5e 98 const float thigh_angle_correction = 3.59;//corrections for the difference in moment arm angle
Technical_Muffin 2:84d479fe9b5e 99 float Mw=M0/g;//M0 is measured in N, but it is calculated as kg in the equation. So instead of changing the whole equation, this is a quick fix.
Technical_Muffin 2:84d479fe9b5e 100 //Calculation of knee torque
Technical_Muffin 2:84d479fe9b5e 101 float Tk = -1*(Ms*g)*(dsm*sin((theta_shin+shank_angle_correction)*PI/180))-((-1*(Ms*g)*(dsm*sin((theta_shin+shank_angle_correction)*PI/180))+(Mw+Marm+Mt+Ms)*g*(ds*sin(theta_shin*PI/180))-(Mt*g)*(dtm*sin((theta_hip-thigh_angle_correction)*PI/180))-(Mw+Marm)*g*(dt*sin(theta_hip*PI/180)))/(ds*cos(theta_shin*PI/180)+dt*cos(theta_hip*PI/180)))*(ds*cos(theta_shin*PI/180))+(Mw+Marm+Mt+Ms)*g*(ds*sin(theta_shin*PI/180));
Technical_Muffin 2:84d479fe9b5e 102
Technical_Muffin 2:84d479fe9b5e 103 return Tk;
Technical_Muffin 2:84d479fe9b5e 104 }
megrootens 0:3855d4588f76 105 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 106 // IMPLEMENTATION USER IO
megrootens 0:3855d4588f76 107 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 108
cnckiwi31 4:1cdce6c6c94e 109 void ResetForce()
cnckiwi31 4:1cdce6c6c94e 110 {
cnckiwi31 4:1cdce6c6c94e 111 leg.nullForce();
cnckiwi31 4:1cdce6c6c94e 112 }
cnckiwi31 4:1cdce6c6c94e 113
cnckiwi31 4:1cdce6c6c94e 114 void ResetPress()
cnckiwi31 4:1cdce6c6c94e 115 {
cnckiwi31 4:1cdce6c6c94e 116 leg.nullPressure0();
cnckiwi31 4:1cdce6c6c94e 117 leg.nullPressure1();
cnckiwi31 4:1cdce6c6c94e 118 }
cnckiwi31 4:1cdce6c6c94e 119
megrootens 0:3855d4588f76 120 bool is_printing = false;
megrootens 0:3855d4588f76 121 void TogglePrinting()
megrootens 0:3855d4588f76 122 {
megrootens 0:3855d4588f76 123 if (not is_printing) {
megrootens 0:3855d4588f76 124 is_printing = true;
megrootens 0:3855d4588f76 125 } else {
megrootens 0:3855d4588f76 126 is_printing = false;
megrootens 0:3855d4588f76 127 PrintMenu();
megrootens 0:3855d4588f76 128 }
megrootens 0:3855d4588f76 129 }
megrootens 0:3855d4588f76 130
megrootens 0:3855d4588f76 131 bool is_logging = false;
megrootens 0:3855d4588f76 132 void ToggleLogging()
megrootens 0:3855d4588f76 133 {
megrootens 0:3855d4588f76 134 if (not is_logging) {
megrootens 0:3855d4588f76 135 StartLogging();
megrootens 0:3855d4588f76 136 } else {
megrootens 0:3855d4588f76 137 is_logging = false;
megrootens 0:3855d4588f76 138 StopLogging();
megrootens 0:3855d4588f76 139 }
megrootens 0:3855d4588f76 140 PrintMenu();
megrootens 0:3855d4588f76 141 }
megrootens 0:3855d4588f76 142
megrootens 0:3855d4588f76 143 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 144 // IMPLEMENTATION SERIAL COM
megrootens 0:3855d4588f76 145 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 146 const int kNumJoints = 4;
cnckiwi31 4:1cdce6c6c94e 147 const char *kJointNames[kNumJoints] = {"Toes","Ankle","Knee","Hip"};
cnckiwi31 4:1cdce6c6c94e 148
cnckiwi31 4:1cdce6c6c94e 149 bool sd_card_present = false;
cnckiwi31 4:1cdce6c6c94e 150 int fname_prepend = 0;
Technical_Muffin 2:84d479fe9b5e 151
megrootens 0:3855d4588f76 152 void PrintStatus()
megrootens 0:3855d4588f76 153 {
megrootens 0:3855d4588f76 154 led_g = !led_g;
megrootens 0:3855d4588f76 155 if (is_printing) {
megrootens 0:3855d4588f76 156 pc.printf("\r\nLEG STATUS (%s)\r\n",led_g?"+":"*");
cnckiwi31 4:1cdce6c6c94e 157 pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No");
cnckiwi31 4:1cdce6c6c94e 158 if(sd_card_present)
cnckiwi31 4:1cdce6c6c94e 159 {
cnckiwi31 4:1cdce6c6c94e 160 pc.printf("\tFile number %15i\r\n", fname_prepend);
cnckiwi31 4:1cdce6c6c94e 161 pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
cnckiwi31 4:1cdce6c6c94e 162 }
cnckiwi31 4:1cdce6c6c94e 163 pc.printf("\n\t%15s %7.2f\r\n","Pressure0 (kPa)", leg.getPressure0()*100);
cnckiwi31 4:1cdce6c6c94e 164 pc.printf("\t%15s %7.2f\r\n","Pressure1 (kPa)", leg.getPressure1()*100);
cnckiwi31 4:1cdce6c6c94e 165
megrootens 0:3855d4588f76 166 for (int i=0; i<kNumJoints; ++i)
cnckiwi31 4:1cdce6c6c94e 167 {
cnckiwi31 4:1cdce6c6c94e 168 string jointName = kJointNames[i];
cnckiwi31 4:1cdce6c6c94e 169 jointName = jointName + " (deg)";
cnckiwi31 4:1cdce6c6c94e 170 pc.printf("\t%15s %7.2f\r\n",jointName, leg.getDegrees(i));
cnckiwi31 4:1cdce6c6c94e 171 }
cnckiwi31 4:1cdce6c6c94e 172 pc.printf("\t%15s %7.2f\r\n","Force (N)", leg.getForce());
cnckiwi31 4:1cdce6c6c94e 173 pc.printf("\t%15s %7.2f\r\n","Torque (Nm)", getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()));
megrootens 0:3855d4588f76 174 }
megrootens 0:3855d4588f76 175 }
megrootens 0:3855d4588f76 176
megrootens 0:3855d4588f76 177 void PrintMenu()
megrootens 0:3855d4588f76 178 {
megrootens 0:3855d4588f76 179 pc.printf("\r\nMENU\r\n");
megrootens 0:3855d4588f76 180 pc.printf("\t> Press SW2 to toggle printing leg status\r\n");
megrootens 0:3855d4588f76 181 pc.printf("\t> Press SW3 to toggle data logging\r\n");
cnckiwi31 4:1cdce6c6c94e 182
cnckiwi31 4:1cdce6c6c94e 183 pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No");
cnckiwi31 4:1cdce6c6c94e 184 if(sd_card_present)
cnckiwi31 4:1cdce6c6c94e 185 {
cnckiwi31 4:1cdce6c6c94e 186 pc.printf("\tFile number %15i\r\n", fname_prepend);
cnckiwi31 4:1cdce6c6c94e 187 pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
cnckiwi31 4:1cdce6c6c94e 188 }
megrootens 0:3855d4588f76 189 }
megrootens 0:3855d4588f76 190
megrootens 0:3855d4588f76 191 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 192 // IMPLEMENTATION DATA LOGGING
megrootens 0:3855d4588f76 193 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 194
megrootens 0:3855d4588f76 195 FILE * fp_data;
megrootens 0:3855d4588f76 196
cnckiwi31 4:1cdce6c6c94e 197
megrootens 0:3855d4588f76 198
megrootens 0:3855d4588f76 199 /**
megrootens 0:3855d4588f76 200 * Check contents of SD card and count files in order
megrootens 0:3855d4588f76 201 * to ensure unique file name for logging data
megrootens 0:3855d4588f76 202 */
megrootens 0:3855d4588f76 203 void InitSdCard()
megrootens 0:3855d4588f76 204 {
megrootens 0:3855d4588f76 205 pc.printf("INITIALIZING SD CARD\r\n");
megrootens 0:3855d4588f76 206
megrootens 0:3855d4588f76 207 int num_files = 0;
megrootens 0:3855d4588f76 208
megrootens 0:3855d4588f76 209 // scan dir
megrootens 0:3855d4588f76 210 DIR *d;
megrootens 0:3855d4588f76 211 struct dirent *p;
megrootens 0:3855d4588f76 212
megrootens 0:3855d4588f76 213 d = opendir("/sd");
megrootens 0:3855d4588f76 214 if (d != NULL) {
megrootens 0:3855d4588f76 215 sd_card_present = true;
megrootens 0:3855d4588f76 216
megrootens 0:3855d4588f76 217 pc.printf("\t> Contents of SD Card:");
megrootens 0:3855d4588f76 218 while ((p = readdir(d)) != NULL) {
megrootens 0:3855d4588f76 219 if (p->d_name[0] != '.') {
megrootens 0:3855d4588f76 220 // skip files starting with '.'
megrootens 0:3855d4588f76 221 pc.printf("\t %s",p->d_name);
megrootens 0:3855d4588f76 222 ++num_files;
megrootens 0:3855d4588f76 223 }
megrootens 0:3855d4588f76 224 }
megrootens 0:3855d4588f76 225 pc.printf("\t> Counted %d visible files.\r\n",num_files);
megrootens 0:3855d4588f76 226
megrootens 0:3855d4588f76 227 closedir(d);
megrootens 0:3855d4588f76 228 } else {
megrootens 0:3855d4588f76 229 sd_card_present = false;
megrootens 0:3855d4588f76 230
megrootens 0:3855d4588f76 231 pc.printf("\t> No SD Card present. Data cannot be logged.\r\n");
megrootens 0:3855d4588f76 232 }
megrootens 0:3855d4588f76 233
megrootens 0:3855d4588f76 234 // id to be appended to logged data files
megrootens 0:3855d4588f76 235 fname_prepend = num_files;
megrootens 0:3855d4588f76 236 }
megrootens 0:3855d4588f76 237
megrootens 0:3855d4588f76 238 /**
megrootens 0:3855d4588f76 239 * Start logging data
megrootens 0:3855d4588f76 240 */
megrootens 0:3855d4588f76 241 void StartLogging(const char * fname_append)
megrootens 0:3855d4588f76 242 {
megrootens 0:3855d4588f76 243
megrootens 0:3855d4588f76 244 pc.printf("DATA LOGGING");
megrootens 0:3855d4588f76 245 if (sd_card_present) {
megrootens 0:3855d4588f76 246
megrootens 0:3855d4588f76 247 // create unique file name
megrootens 0:3855d4588f76 248 ++fname_prepend;
megrootens 0:3855d4588f76 249 char fname[50];
megrootens 0:3855d4588f76 250 sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append);
megrootens 0:3855d4588f76 251
megrootens 0:3855d4588f76 252 pc.printf("\t> Opening data log file '%s'...\r\n",fname);
megrootens 0:3855d4588f76 253
megrootens 0:3855d4588f76 254 // open file for writing and start logging after success
megrootens 0:3855d4588f76 255 fp_data = fopen(fname,"w");
megrootens 0:3855d4588f76 256 if (fp_data==NULL) {
megrootens 0:3855d4588f76 257 pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
megrootens 0:3855d4588f76 258 timer.read_ms());
megrootens 0:3855d4588f76 259 } else {
cnckiwi31 4:1cdce6c6c94e 260 fprintf(fp_data, "time (s), theta_toe (deg), theta_ankle (deg), theta_knee (deg), theta_hip (deg), force (N), torque (Nm), pressure0 (kPa), pressure1 (kPa)");
megrootens 0:3855d4588f76 261 tick_logging.attach_us(&LogData,timing::kTimeLogDataUs);
megrootens 1:417a5b28ac84 262 timer.start();
cnckiwi31 4:1cdce6c6c94e 263 startedLogging = true;
cnckiwi31 4:1cdce6c6c94e 264
megrootens 0:3855d4588f76 265 pc.printf("\t> Logging started.\r\n");
megrootens 0:3855d4588f76 266
megrootens 0:3855d4588f76 267 is_logging = true;
megrootens 0:3855d4588f76 268 }
megrootens 0:3855d4588f76 269
megrootens 0:3855d4588f76 270 } else {
megrootens 0:3855d4588f76 271 pc.printf("\t> No SD Card; no data will be logged.\r\n");
cnckiwi31 4:1cdce6c6c94e 272 startedLogging = false;
megrootens 0:3855d4588f76 273 }
megrootens 0:3855d4588f76 274 }
megrootens 0:3855d4588f76 275
megrootens 0:3855d4588f76 276
megrootens 0:3855d4588f76 277 /**
megrootens 0:3855d4588f76 278 * Stop logging data
megrootens 0:3855d4588f76 279 */
megrootens 0:3855d4588f76 280 void StopLogging()
megrootens 0:3855d4588f76 281 {
megrootens 0:3855d4588f76 282 pc.printf("DATA LOGGING:");
megrootens 0:3855d4588f76 283 if (sd_card_present) {
megrootens 0:3855d4588f76 284 // close data file, stop logging
megrootens 0:3855d4588f76 285 fclose(fp_data);
megrootens 0:3855d4588f76 286 tick_logging.detach();
megrootens 1:417a5b28ac84 287 timer.stop();
megrootens 1:417a5b28ac84 288 timer.reset();
megrootens 0:3855d4588f76 289 pc.printf("\r> Stopped.");
megrootens 0:3855d4588f76 290 } else {
megrootens 0:3855d4588f76 291 pc.printf("\t> No data was logged.");
megrootens 0:3855d4588f76 292 }
megrootens 0:3855d4588f76 293
megrootens 0:3855d4588f76 294 is_logging = false;
megrootens 0:3855d4588f76 295 }
megrootens 0:3855d4588f76 296 /**
megrootens 0:3855d4588f76 297 * Log data
megrootens 0:3855d4588f76 298 */
megrootens 0:3855d4588f76 299 void LogData()
megrootens 0:3855d4588f76 300 {
cnckiwi31 4:1cdce6c6c94e 301 int currTime = timer.read_ms();
cnckiwi31 4:1cdce6c6c94e 302 if(startedLogging) {
cnckiwi31 4:1cdce6c6c94e 303 firstReadMS = currTime;
cnckiwi31 4:1cdce6c6c94e 304 startedLogging = false;
cnckiwi31 4:1cdce6c6c94e 305 }
cnckiwi31 4:1cdce6c6c94e 306 double currTimeS = ((double)currTime - firstReadMS)/1000;
cnckiwi31 4:1cdce6c6c94e 307
megrootens 0:3855d4588f76 308 // time
cnckiwi31 4:1cdce6c6c94e 309 fprintf(fp_data,"\n%+f",currTimeS);
megrootens 0:3855d4588f76 310
megrootens 0:3855d4588f76 311 // bench: joint angles and force sensor
cnckiwi31 4:1cdce6c6c94e 312 fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f, %+f, %+f, %+f",
megrootens 0:3855d4588f76 313 leg.getDegrees(0),
megrootens 0:3855d4588f76 314 leg.getDegrees(1),
megrootens 0:3855d4588f76 315 leg.getDegrees(2),
megrootens 0:3855d4588f76 316 leg.getDegrees(3),
Technical_Muffin 2:84d479fe9b5e 317 leg.getForce(),
cnckiwi31 4:1cdce6c6c94e 318 getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()),
cnckiwi31 4:1cdce6c6c94e 319 leg.getPressure0()*100,
cnckiwi31 4:1cdce6c6c94e 320 leg.getPressure1()*100
megrootens 0:3855d4588f76 321 );
megrootens 0:3855d4588f76 322 }