Ben Katz / SPIne

SPI to quad CAN firmware

Dependencies:   mbed-dev_spine

Fork of Teleop_Controller by Ben Katz

main.cpp@1:79e0d4791936, 2017-11-18 (annotated)

Committer:
benkatz
Date:
Sat Nov 18 19:42:18 2017 +0000
Revision:
1:79e0d4791936
Parent:
0:d6186b8990c5
Child:
2:25837cbaee98
working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:d6186b8990c5 1
benkatz 1:79e0d4791936 2 #define CAN_ID 0x0
benkatz 0:d6186b8990c5 3
benkatz 0:d6186b8990c5 4 #include "mbed.h"
benkatz 0:d6186b8990c5 5 #include "math_ops.h"
benkatz 0:d6186b8990c5 6
benkatz 0:d6186b8990c5 7
benkatz 0:d6186b8990c5 8 Serial pc(PA_2, PA_3);
benkatz 1:79e0d4791936 9 CAN can1(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 1:79e0d4791936 10 CAN can2(PB_5, PB_13); // CAN Rx pin name, CAN Tx pin name
benkatz 1:79e0d4791936 11 CANMessage rxMsg1, rxMsg2;
benkatz 1:79e0d4791936 12 CANMessage abad1, abad2, hip1, hip2, knee1, knee2; //TX Messages
benkatz 0:d6186b8990c5 13 int ledState;
benkatz 0:d6186b8990c5 14 Ticker sendCAN;
benkatz 0:d6186b8990c5 15 int counter = 0;
benkatz 0:d6186b8990c5 16 volatile bool msgAvailable = false;
benkatz 0:d6186b8990c5 17 Ticker loop;
benkatz 0:d6186b8990c5 18
benkatz 1:79e0d4791936 19 ///[[abad1, abad2]
benkatz 1:79e0d4791936 20 ///[hip1, hip2]
benkatz 1:79e0d4791936 21 ///[knee1, knee2]]
benkatz 1:79e0d4791936 22 float q[3][2]; //Joint states for both legs
benkatz 1:79e0d4791936 23 float dq[3][2];
benkatz 1:79e0d4791936 24 float tau[3][3];
benkatz 1:79e0d4791936 25 float kp = 60;
benkatz 1:79e0d4791936 26 float kd = 0.8;
benkatz 1:79e0d4791936 27 int enabled = 0;
benkatz 0:d6186b8990c5 28
benkatz 0:d6186b8990c5 29 /// Value Limits ///
benkatz 0:d6186b8990c5 30 #define P_MIN -12.5f
benkatz 0:d6186b8990c5 31 #define P_MAX 12.5f
benkatz 0:d6186b8990c5 32 #define V_MIN -30.0f
benkatz 0:d6186b8990c5 33 #define V_MAX 30.0f
benkatz 0:d6186b8990c5 34 #define KP_MIN 0.0f
benkatz 0:d6186b8990c5 35 #define KP_MAX 500.0f
benkatz 0:d6186b8990c5 36 #define KD_MIN 0.0f
benkatz 0:d6186b8990c5 37 #define KD_MAX 5.0f
benkatz 0:d6186b8990c5 38 #define T_MIN -18.0f
benkatz 0:d6186b8990c5 39 #define T_MAX 18.0f
benkatz 0:d6186b8990c5 40 #define I_MAX 40.0f
benkatz 0:d6186b8990c5 41
benkatz 0:d6186b8990c5 42 /// CAN Command Packet Structure ///
benkatz 0:d6186b8990c5 43 /// 16 bit position command, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 44 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 45 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 0:d6186b8990c5 46 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 0:d6186b8990c5 47 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 0:d6186b8990c5 48 /// CAN Packet is 8 8-bit words
benkatz 0:d6186b8990c5 49 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 50 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 51 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 52 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 53 /// 3: [velocity[3-0], kp[11-8]]
benkatz 0:d6186b8990c5 54 /// 4: [kp[7-0]]
benkatz 0:d6186b8990c5 55 /// 5: [kd[11-4]]
benkatz 0:d6186b8990c5 56 /// 6: [kd[3-0], torque[11-8]]
benkatz 0:d6186b8990c5 57 /// 7: [torque[7-0]]
benkatz 0:d6186b8990c5 58
benkatz 0:d6186b8990c5 59 void pack_cmd(CANMessage * msg, float p_des, float v_des, float kp, float kd, float t_ff){
benkatz 0:d6186b8990c5 60 /// limit data to be within bounds ///
benkatz 0:d6186b8990c5 61 p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);
benkatz 0:d6186b8990c5 62 v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
benkatz 0:d6186b8990c5 63 kp = fminf(fmaxf(KP_MIN, kp), KP_MAX);
benkatz 0:d6186b8990c5 64 kd = fminf(fmaxf(KD_MIN, kd), KD_MAX);
benkatz 0:d6186b8990c5 65 t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX);
benkatz 0:d6186b8990c5 66 /// convert floats to unsigned ints ///
benkatz 0:d6186b8990c5 67 int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 68 int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
benkatz 0:d6186b8990c5 69 int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
benkatz 0:d6186b8990c5 70 int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
benkatz 0:d6186b8990c5 71 int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
benkatz 0:d6186b8990c5 72 /// pack ints into the can buffer ///
benkatz 0:d6186b8990c5 73 msg->data[0] = p_int>>8;
benkatz 0:d6186b8990c5 74 msg->data[1] = p_int&0xFF;
benkatz 0:d6186b8990c5 75 msg->data[2] = v_int>>4;
benkatz 0:d6186b8990c5 76 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
benkatz 0:d6186b8990c5 77 msg->data[4] = kp_int&0xFF;
benkatz 0:d6186b8990c5 78 msg->data[5] = kd_int>>4;
benkatz 0:d6186b8990c5 79 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
benkatz 0:d6186b8990c5 80 msg->data[7] = t_int&0xff;
benkatz 0:d6186b8990c5 81 }
benkatz 0:d6186b8990c5 82
benkatz 0:d6186b8990c5 83 /// CAN Reply Packet Structure ///
benkatz 0:d6186b8990c5 84 /// 16 bit position, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 85 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 86 /// 12 bit current, between -40 and 40;
benkatz 0:d6186b8990c5 87 /// CAN Packet is 5 8-bit words
benkatz 0:d6186b8990c5 88 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 89 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 90 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 91 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 92 /// 3: [velocity[3-0], current[11-8]]
benkatz 0:d6186b8990c5 93 /// 4: [current[7-0]]
benkatz 0:d6186b8990c5 94
benkatz 1:79e0d4791936 95 void unpack_reply(CANMessage msg, int leg_num){
benkatz 0:d6186b8990c5 96 /// unpack ints from can buffer ///
benkatz 0:d6186b8990c5 97 int id = msg.data[0];
benkatz 0:d6186b8990c5 98 int p_int = (msg.data[1]<<8)|msg.data[2];
benkatz 0:d6186b8990c5 99 int v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
benkatz 0:d6186b8990c5 100 int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
benkatz 0:d6186b8990c5 101 /// convert ints to floats ///
benkatz 0:d6186b8990c5 102 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 103 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
benkatz 1:79e0d4791936 104 float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
benkatz 0:d6186b8990c5 105
benkatz 1:79e0d4791936 106 q[id-1][leg_num] = p;
benkatz 1:79e0d4791936 107 dq[id-1][leg_num] = v;
benkatz 1:79e0d4791936 108 /*
benkatz 0:d6186b8990c5 109 if(id == 2){
benkatz 0:d6186b8990c5 110 theta1 = p;
benkatz 0:d6186b8990c5 111 dtheta1 = v;
benkatz 0:d6186b8990c5 112 }
benkatz 0:d6186b8990c5 113 else if(id ==3){
benkatz 0:d6186b8990c5 114 theta2 = p;
benkatz 0:d6186b8990c5 115 dtheta2 = v;
benkatz 0:d6186b8990c5 116 }
benkatz 1:79e0d4791936 117 */
benkatz 0:d6186b8990c5 118
benkatz 1:79e0d4791936 119 //printf("%d %.3f %.3f %.3f\n\r", id, p, v, i);
benkatz 0:d6186b8990c5 120 }
benkatz 0:d6186b8990c5 121
benkatz 1:79e0d4791936 122 void rxISR1() {
benkatz 1:79e0d4791936 123 can1.read(rxMsg1); // read message into Rx message storage
benkatz 1:79e0d4791936 124 unpack_reply(rxMsg1, 0);
benkatz 0:d6186b8990c5 125 }
benkatz 1:79e0d4791936 126 void rxISR2(){
benkatz 1:79e0d4791936 127 can2.read(rxMsg2);
benkatz 1:79e0d4791936 128 unpack_reply(rxMsg2, 1);
benkatz 1:79e0d4791936 129 }
benkatz 0:d6186b8990c5 130
benkatz 1:79e0d4791936 131 void WriteAll(){
benkatz 1:79e0d4791936 132 can1.write(abad1);
benkatz 1:79e0d4791936 133 wait(.0001);
benkatz 1:79e0d4791936 134 can2.write(abad2);
benkatz 1:79e0d4791936 135 wait(.0001);
benkatz 1:79e0d4791936 136 can1.write(hip1);
benkatz 1:79e0d4791936 137 wait(.0001);
benkatz 1:79e0d4791936 138 can2.write(hip2);
benkatz 1:79e0d4791936 139 wait(.0001);
benkatz 1:79e0d4791936 140 can1.write(knee1);
benkatz 1:79e0d4791936 141 wait(.0001);
benkatz 1:79e0d4791936 142 can2.write(knee2);
benkatz 1:79e0d4791936 143 wait(.0001);
benkatz 1:79e0d4791936 144 }
benkatz 0:d6186b8990c5 145
benkatz 0:d6186b8990c5 146 void sendCMD(){
benkatz 0:d6186b8990c5 147 /// bilateral teleoperation demo ///
benkatz 1:79e0d4791936 148 counter ++;
benkatz 1:79e0d4791936 149
benkatz 1:79e0d4791936 150 if(enabled){
benkatz 1:79e0d4791936 151 if(counter>100){
benkatz 1:79e0d4791936 152 //tcmd = -1*tcmd;
benkatz 1:79e0d4791936 153 //printf("%.4f %.4f %.4f %.4f %.4f %.4f\n\r", q[0][0], q[1][0], q[2][0], q[0][1], q[1][1], q[2][1]);
benkatz 1:79e0d4791936 154 counter = 0 ;
benkatz 1:79e0d4791936 155 }
benkatz 1:79e0d4791936 156
benkatz 1:79e0d4791936 157 tau[0][1] = kp*(q[0][0] - q[0][1]) + kd*(dq[0][0] - dq[0][1]);
benkatz 1:79e0d4791936 158 tau[0][0] = kp*(q[0][1] - q[0][0]) + kd*(dq[0][1] - dq[0][0]);
benkatz 1:79e0d4791936 159 tau[1][1] = kp*(q[1][0] - q[1][1]) + kd*(dq[1][0] - dq[1][1]);
benkatz 1:79e0d4791936 160 tau[1][0] = kp*(q[1][1] - q[1][0]) + kd*(dq[1][1] - dq[1][0]);
benkatz 1:79e0d4791936 161 tau[2][1] = (kp/1.5f)*(q[2][0] - q[2][1]) + (kd/2.25f)*(dq[2][0] - dq[2][1]);
benkatz 1:79e0d4791936 162 tau[2][0] = (kp/1.5f)*(q[2][1] - q[2][0]) + (kd/2.25f)*(dq[2][1] - dq[2][0]);
benkatz 1:79e0d4791936 163
benkatz 1:79e0d4791936 164 pack_cmd(&abad1, 0, 0, 0, .01, tau[0][0]);
benkatz 1:79e0d4791936 165 pack_cmd(&abad2, 0, 0, 0, .01, tau[0][1]);
benkatz 1:79e0d4791936 166 pack_cmd(&hip1, 0, 0, 0, .01, tau[1][0]);
benkatz 1:79e0d4791936 167 pack_cmd(&hip2, 0, 0, 0, .01, tau[1][1]);
benkatz 1:79e0d4791936 168 pack_cmd(&knee1, 0, 0, 0, .006, tau[2][0]);
benkatz 1:79e0d4791936 169 pack_cmd(&knee2, 0, 0, 0, .006, tau[2][1]);
benkatz 1:79e0d4791936 170 /*
benkatz 1:79e0d4791936 171 pack_cmd(&abad1, q[0][1], dq[0][1], kp, kd, 0);
benkatz 1:79e0d4791936 172 pack_cmd(&abad2, q[0][0], dq[0][0], kp, kd, 0);
benkatz 1:79e0d4791936 173 pack_cmd(&hip1, q[1][1], dq[1][1], kp, kd, 0);
benkatz 1:79e0d4791936 174 pack_cmd(&hip2, q[1][0], dq[1][0], kp, kd, 0);
benkatz 1:79e0d4791936 175 pack_cmd(&knee1, q[2][1], dq[2][1], kp/1.5f, kd/2.25f, 0);
benkatz 1:79e0d4791936 176 pack_cmd(&knee2, q[2][0], dq[2][0], kp/1.5f, kd/2.25f, 0);
benkatz 1:79e0d4791936 177 */
benkatz 1:79e0d4791936 178 }
benkatz 1:79e0d4791936 179 /*
benkatz 1:79e0d4791936 180 pack_cmd(&abad1, 0, 0, 10, .1, 0);
benkatz 1:79e0d4791936 181 pack_cmd(&abad2, 0, 0, 10, .1, 0);
benkatz 1:79e0d4791936 182 pack_cmd(&hip1, 0, 0, 10, .1, 0);
benkatz 1:79e0d4791936 183 pack_cmd(&hip2, 0, 0, 10, .1, 0);
benkatz 1:79e0d4791936 184 pack_cmd(&knee1, 0, 0, 6.6, .04, 0);
benkatz 1:79e0d4791936 185 pack_cmd(&knee2, 0, 0, 6.6, .04, 0);
benkatz 1:79e0d4791936 186 */
benkatz 1:79e0d4791936 187 WriteAll();
benkatz 0:d6186b8990c5 188 }
benkatz 0:d6186b8990c5 189
benkatz 1:79e0d4791936 190 void Zero(CANMessage * msg){
benkatz 1:79e0d4791936 191 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 192 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 193 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 194 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 195 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 196 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 197 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 198 msg->data[7] = 0xFE;
benkatz 1:79e0d4791936 199 WriteAll();
benkatz 1:79e0d4791936 200 }
benkatz 1:79e0d4791936 201
benkatz 1:79e0d4791936 202 void EnterMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 203 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 204 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 205 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 206 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 207 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 208 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 209 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 210 msg->data[7] = 0xFC;
benkatz 1:79e0d4791936 211 WriteAll();
benkatz 1:79e0d4791936 212 }
benkatz 1:79e0d4791936 213
benkatz 1:79e0d4791936 214 void ExitMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 215 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 216 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 217 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 218 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 219 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 220 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 221 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 222 msg->data[7] = 0xFD;
benkatz 1:79e0d4791936 223 WriteAll();
benkatz 1:79e0d4791936 224 }
benkatz 0:d6186b8990c5 225 void serial_isr(){
benkatz 1:79e0d4791936 226 /// handle keyboard commands from the serial terminal ///
benkatz 0:d6186b8990c5 227 while(pc.readable()){
benkatz 0:d6186b8990c5 228 char c = pc.getc();
benkatz 0:d6186b8990c5 229 switch(c){
benkatz 0:d6186b8990c5 230 case(27):
benkatz 1:79e0d4791936 231 loop.detach();
benkatz 0:d6186b8990c5 232 printf("\n\r exiting motor mode \n\r");
benkatz 1:79e0d4791936 233 ExitMotorMode(&abad1);
benkatz 1:79e0d4791936 234 ExitMotorMode(&abad2);
benkatz 1:79e0d4791936 235 ExitMotorMode(&hip1);
benkatz 1:79e0d4791936 236 ExitMotorMode(&hip2);
benkatz 1:79e0d4791936 237 ExitMotorMode(&knee1);
benkatz 1:79e0d4791936 238 ExitMotorMode(&knee2);
benkatz 1:79e0d4791936 239 enabled = 0;
benkatz 0:d6186b8990c5 240 break;
benkatz 0:d6186b8990c5 241 case('m'):
benkatz 0:d6186b8990c5 242 printf("\n\r entering motor mode \n\r");
benkatz 1:79e0d4791936 243 EnterMotorMode(&abad1);
benkatz 1:79e0d4791936 244 Zero(&abad1);
benkatz 1:79e0d4791936 245 EnterMotorMode(&abad2);
benkatz 1:79e0d4791936 246 Zero(&abad2);
benkatz 1:79e0d4791936 247 EnterMotorMode(&hip1);
benkatz 1:79e0d4791936 248 Zero(&hip1);
benkatz 1:79e0d4791936 249 EnterMotorMode(&hip2);
benkatz 1:79e0d4791936 250 Zero(&hip2);
benkatz 1:79e0d4791936 251 EnterMotorMode(&knee1);
benkatz 1:79e0d4791936 252 Zero(&knee1);
benkatz 1:79e0d4791936 253 EnterMotorMode(&knee2);
benkatz 1:79e0d4791936 254 Zero(&knee2);
benkatz 1:79e0d4791936 255 wait(.5);
benkatz 1:79e0d4791936 256 enabled = 1;
benkatz 1:79e0d4791936 257 loop.attach(&sendCMD, .001);
benkatz 0:d6186b8990c5 258 break;
benkatz 0:d6186b8990c5 259 case('z'):
benkatz 0:d6186b8990c5 260 printf("\n\r zeroing \n\r");
benkatz 1:79e0d4791936 261 Zero(&abad1);
benkatz 1:79e0d4791936 262 Zero(&abad2);
benkatz 1:79e0d4791936 263 Zero(&hip1);
benkatz 1:79e0d4791936 264 Zero(&hip2);
benkatz 1:79e0d4791936 265 Zero(&knee1);
benkatz 1:79e0d4791936 266 Zero(&knee2);
benkatz 0:d6186b8990c5 267 break;
benkatz 0:d6186b8990c5 268 }
benkatz 0:d6186b8990c5 269 }
benkatz 1:79e0d4791936 270 WriteAll();
benkatz 0:d6186b8990c5 271
benkatz 0:d6186b8990c5 272 }
benkatz 0:d6186b8990c5 273
benkatz 0:d6186b8990c5 274 int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8};
benkatz 0:d6186b8990c5 275 int main() {
benkatz 0:d6186b8990c5 276 pc.baud(921600);
benkatz 0:d6186b8990c5 277 pc.attach(&serial_isr);
benkatz 1:79e0d4791936 278 can1.frequency(1000000); // set bit rate to 1Mbps
benkatz 1:79e0d4791936 279 can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler
benkatz 1:79e0d4791936 280 can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 1:79e0d4791936 281 can2.frequency(1000000); // set bit rate to 1Mbps
benkatz 1:79e0d4791936 282 can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler
benkatz 1:79e0d4791936 283 can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 1:79e0d4791936 284
benkatz 1:79e0d4791936 285 printf("\n\r Master\n\r");
benkatz 0:d6186b8990c5 286 //printf("%d\n\r", RX_ID << 18);
benkatz 1:79e0d4791936 287 abad1.len = 8; //transmit 8 bytes
benkatz 1:79e0d4791936 288 abad2.len = 8; //transmit 8 bytes
benkatz 1:79e0d4791936 289 hip1.len = 8;
benkatz 1:79e0d4791936 290 hip2.len = 8;
benkatz 1:79e0d4791936 291 knee1.len = 8;
benkatz 1:79e0d4791936 292 knee2.len = 8;
benkatz 1:79e0d4791936 293 rxMsg1.len = 6; //receive 5 bytes
benkatz 1:79e0d4791936 294 rxMsg2.len = 6; //receive 5 bytes
benkatz 1:79e0d4791936 295
benkatz 1:79e0d4791936 296
benkatz 1:79e0d4791936 297 abad1.id = 0x1;
benkatz 1:79e0d4791936 298 abad2.id = 0x1;
benkatz 1:79e0d4791936 299 hip1.id = 0x2;
benkatz 1:79e0d4791936 300 hip2.id = 0x2;
benkatz 1:79e0d4791936 301 knee1.id = 0x3;
benkatz 1:79e0d4791936 302 knee2.id = 0x3;
benkatz 1:79e0d4791936 303 pack_cmd(&abad1, 0, 0, 0, 0, 0); //Start out by sending all 0's
benkatz 1:79e0d4791936 304 pack_cmd(&abad2, 0, 0, 0, 0, 0);
benkatz 1:79e0d4791936 305 pack_cmd(&hip1, 0, 0, 0, 0, 0);
benkatz 1:79e0d4791936 306 pack_cmd(&hip2, 0, 0, 0, 0, 0);
benkatz 1:79e0d4791936 307 pack_cmd(&knee1, 0, 0, 0, 0, 0);
benkatz 1:79e0d4791936 308 pack_cmd(&knee2, 0, 0, 0, 0, 0);
benkatz 1:79e0d4791936 309
benkatz 0:d6186b8990c5 310
benkatz 0:d6186b8990c5 311 while(1) {
benkatz 0:d6186b8990c5 312
benkatz 0:d6186b8990c5 313 }
benkatz 0:d6186b8990c5 314
benkatz 0:d6186b8990c5 315 }
benkatz 0:d6186b8990c5 316
benkatz 0:d6186b8990c5 317
benkatz 0:d6186b8990c5 318
benkatz 0:d6186b8990c5 319
benkatz 0:d6186b8990c5 320