Allan Veale / Mbed 2 deprecated heros_leg_readout_torque_addition

HeRoS: read out and log joint angles and force sensor data from the leg test bench. Now with additional features to read pressure sensors and set the null values of the pressure and force sensors

Dependencies:   SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect Valve LCM101

Fork of heros_leg_readout_torque_addition by K K

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 }