├── README.md ├── all_movement_functions └── all_movement_functions.ino ├── hexapod_class ├── hexapod_lib.cpp └── hexapod_lib.h ├── images ├── apply_to_other_legs.png ├── apply_to_other_legs2.png ├── body_frame.png ├── dh_parameters_definition.png ├── fwd_kin.png ├── hexapod_actual.png ├── inplace_turning.png ├── inv_kin.png ├── propo.png ├── servo_angle_mapping.png ├── single_leg_frames.png ├── single_leg_motion_walking.png ├── single_leg_move_example.png ├── steering.png └── wiring_diagram.png └── test_codes ├── input_serial_pwm └── input_serial_pwm.ino ├── test_bno055 └── test_bno055.ino ├── test_class └── test_class.ino ├── test_drive_servo └── test_drive_servo.ino ├── test_drive_servo_0x41 └── test_drive_servo_0x41.ino ├── test_first_second_legs └── test_first_second_legs.ino ├── test_ijn_legs └── test_ijn_legs.ino ├── test_normal_walking └── test_normal_walking.ino ├── test_one_leg └── test_one_leg.ino ├── test_print_inplace_turning └── test_print_inplace_turning.ino ├── test_rotate_translate └── test_rotate_translate.ino ├── test_sbus_drive_walking └── test_sbus_drive_walking.ino └── test_walking └── test_walking.ino /README.md: -------------------------------------------------------------------------------- 1 | # Hexapod class 2 | 3 | This hexapod class is to use with ESP32 dev kit in Arduino framework. 4 | 5 | ![](images/hexapod_actual.png) 6 | 7 | The frame is from 3D printed parts, and the actuators are just RC servo as Futaba A700 and DS3225. 8 | 9 | ## Concept 10 | 11 | Coordinate frame O is placed on the center body where $`Y_O`$ is the pointing forward direction, and $`X_O`$ is pointing rigth-sided direction of robot would be going. The legs notation is used as leg1, leg2, leg3, leg4, leg5, and leg6, or sometimes as leg i, leg j, leg k, leg l, leg m and leg n interchangeably. 12 | 13 | The kinematics and foot motion will be thought on leg 1 or leg i frame, and then other legs could get the result by applying the homogenous transformation according to each leg's frame. 14 | 15 | ![](images/body_frame.png) 16 | 17 | Let's see just a single leg kinematics first. 18 | 19 | ### Forward Kinametics 20 | 21 | Here we look only just leg i, the coordinate frame of each joint is shown. The joint coordinate frame needs to be place carefully otherwise the DH-parameters would be incorrect. The coxa link has link lenght as $`L_1`$, femur leg as $`L_2`$ and tibia link as $`L_3`$. The joint angle offset $`\alpha_1`$ of coxa link is 90 degrees. 22 | 23 | ![](images/single_leg_frames.png) 24 | 25 | The way to fill up DH-parameters on the table is refered to the text book from ROBOT ANALYSIS The Mechanics of Serial and Parallel Manipulators from LUNG-WEN TSAI. 26 | 27 | ![](images/dh_parameters_definition.png) 28 | 29 | Then we can create transformation matrices of $`{}^{0}_{}T^{}_{1}`$, $`{}^{1}_{}T^{}_{2}`$, $`{}^{2}_{}T^{}_{3}`$, and $`{}^{0}_{}T^{}_{3}`$ as following. 30 | 31 | ![](images/fwd_kin.png) 32 | 33 | For example, when all of $`\theta_1`$, $`\theta_2`$ and $`\theta_3`$ are zero, then the leg will be strecth out fully. And as my design, the tibia link is not flipable so most of the time $`\theta_3`$ will be negative number because it's alway pointing a foot down. 34 | 35 | ![](images/single_leg_move_example.png) 36 | 37 | The last column of $`{}^{0}_{}T^{}_{3}`$ is showing X, Y, Z of foot position according to $`0^th`$ frame. 38 | 39 | ### Inverse Kinematics 40 | 41 | I am using analytical method to solve inverse kinematics. The diagram below is showing a posture where $`\theta_1`$, $`\theta_2`$ and $`\theta_3`$ are not zero and the foot is pointing to the ground. If $`X_p, Y_p, Z_p`$ of foot position are given, then $`\theta_1`$, $`\theta_2`$ and $`\theta_3`$ could be calculate from the following equations. The femur and tibia links are formed as triangle $`L_2 L_3 r_1`$ so we could use cosine rules to solve angles of $`\phi_1`$, $`\phi_2`$ and $`\phi_3`$ which will give the result of desired joint angle of $`\theta_1`$, $`\theta_2`$ and $`\theta_3`$. 42 | 43 | ![](images/inv_kin.png) 44 | 45 | ### Foot motion for walking 46 | 47 | In order to make the hexapod walk, we're using only tripple gait pattern (most common), where three legs are on ground and other three legs are lifting up. 48 | 49 | So the foot motion is divided to two phases as stance (line) and swing (curve) phases. 50 | 51 | We can think about this as on $`0^th`$ frame of the leg. The stance phase is just a simple line and it depends on which direction to go then the point starts from positive or negative sides. The swing phase is a curve line which has the same end point of stance phase. This curve line is known as Bezier's curve where you define three points and you will get the parametic equations of horizontal and vertical axes. 52 | 53 | In this case, the stance phase is defined by parameter $`T`$. $`S`$ is a height offset from origin point, and $`A`$ is the height of step. From these three parameters, we can have three points as $`P_1`$, $`P_2`$ and $`P_3`$. The line and curve equation can be made as following. 54 | 55 | ![](images/single_leg_motion_walking.png) 56 | 57 | These points we got are in Y-Z plane, so depends on how far the leg should be strecth, we decide the X data point according to that. 58 | 59 | To apply these point to other leg frame, we could think these points are firstly in O-frame (body static frame), 60 | 61 | ![](images/apply_to_other_legs.png) 62 | 63 | then we can apply a transformation to each leg depends on the rotation and offset distance of each leg frame. In case of robot would be walking forward ($`Y_O`$ direction), then the points in each leg should be as in the picture below. 64 | 65 | ![](images/apply_to_other_legs2.png) 66 | 67 | If we rotate these points around itself, then we can have a crab-walking motion where the robot could walk along any direction. 68 | 69 | To make the robot turning in-place, we can make up a simple rule of leg motion as following, then leg i, leg k, and leg m would have the same motion, and leg j, leg l, and leg n would have the same motion. 70 | 71 | ![](images/inplace_turning.png) 72 | 73 | To make the robot walking like car steering, we could imagine the robot is turning with the radius of $`R_{ICC}`$, this idea is similar to differential drive mobile robot where it's moving in circular path. The inner legs of the circle will have shorter range, and outer legs of circlr will have bigger range. 74 | 75 | ![](images/steering.png) 76 | 77 | Let's keep the leg i path as a guide line, so the stance phase will now not only just a straight line but an arc of circular path. The arc angle is $`\Phi`$, and because all of the legs is turning around $`O_{ICC}`$ point as circle, so each leg should move within the same arc angle $`\Phi`$, but as each leg has different radius, so the path is not same for all. But once we calculate the guide line path of leg i, then we can use the same technique of homogenous transformation to transform these points into each leg frame. 78 | 79 | ## Implementation 80 | 81 | The microcontroller is ESP32 Dev kit C type. The servo drive is Adafruit 12-channels PWM driver, and we use two of that. To control the robot wirelessly, I am using Futaba radio controller, so on the receiver side we need to parse SBUS data which comes from UART2 port. And the BNO055 IMU for self leveling function. 82 | 83 | ![](images/wiring_diagram.png) 84 | 85 | The servo on joint 1, I used Futaba A700 because I already have a bunch of this servo from other project, so I reuse that. For joint 2 and 3, I use DS3225 a cheap 20kg.cm servo. The servo angle and kinematics angles are different in each joint and each servo, so the table below is showing the correc mapping value of PWM value, servo angle (in degrees) and joint kinematics angle (in degrees). 86 | 87 | ![](images/servo_angle_mapping.png) 88 | 89 | To operate the robot, the radio channels on Futaba Tx are assigned. 90 | 91 | ![](images/propo.png) -------------------------------------------------------------------------------- /all_movement_functions/all_movement_functions.ino: -------------------------------------------------------------------------------- 1 | #include "driver/uart.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire); 11 | 12 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 13 | 14 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); 15 | Adafruit_PWMServoDriver pwm_2 = Adafruit_PWMServoDriver(0x41); 16 | 17 | HexapodLib h; 18 | 19 | /////////////////////// 20 | /// SBUS PARSER ISR /// 21 | /////////////////////// 22 | /// Parser Thread 23 | #define UART_PORT_NUM UART_NUM_2 24 | #define BUF_SIZE (1024 * 2) 25 | #define RD_BUF_SIZE (1024) 26 | static QueueHandle_t uart2_queue; 27 | static const char * TAG = ""; 28 | #define U2RXD 16 29 | #define U2TXD 17 30 | uint8_t rxbuf[256]; 31 | uint16_t rx_fifo_len; 32 | uint16_t ch[16]; 33 | uint16_t checksum = 0; 34 | 35 | /// Main Thread 36 | uint16_t sbus_ch[16]; 37 | uint16_t ch1; 38 | uint16_t ch2; 39 | uint16_t ch4; 40 | uint16_t ch5; 41 | uint16_t ch7; 42 | uint16_t ch8; 43 | int sbus_max = 1680; 44 | int sbus_mid = 1024; 45 | int sbus_min = 368; 46 | int sbus_db_max = 1072; 47 | int sbus_db_min = 976; 48 | 49 | ///////////////// 50 | /// Hexapod /// 51 | //////////////// 52 | int delay_time = 25; 53 | int walk_counter = 0; 54 | int turn_counter = 0; 55 | int ang_inc = 0; 56 | int str_index = 7; 57 | int turn_inc = 0; 58 | unsigned long period = 0; 59 | unsigned long last_stamp; 60 | unsigned long last_home_stamp; 61 | bool do_smooth = false; 62 | bool stick_ch1_pressed = false; 63 | bool stick_ch2_pressed = false; 64 | bool stick_ch4_pressed = false; 65 | bool stick_ch5_pressed = false; 66 | bool sticks_pressed = false; 67 | int max_delay = 50; 68 | int min_delay = 10; 69 | int default_delay = 25; 70 | bool from_walk = false; 71 | bool start_drive = false; 72 | 73 | // mode // 74 | char *robot_mode[] = {"NORM", "CRAB", "BODY"}; 75 | int robot_mode_index = 0; 76 | 77 | // translation & rotatioon // 78 | float x_trans; 79 | float y_trans; 80 | float z_trans; 81 | float roll; 82 | float pitch; 83 | float yaw; 84 | float leg1_XYZ[3]; 85 | float leg2_XYZ[3]; 86 | float leg3_XYZ[3]; 87 | float leg4_XYZ[3]; 88 | float leg5_XYZ[3]; 89 | float leg6_XYZ[3]; 90 | 91 | ////////////// 92 | /// BNO055 /// 93 | ////////////// 94 | float qw; 95 | float qx; 96 | float qy; 97 | float qz; 98 | float r11, r12, r13, r21, r22, r23, r31, r32, r33; 99 | float imu_roll, imu_pitch, body_yaw; 100 | float comp_roll, comp_pitch; 101 | 102 | /// PID /// 103 | double Setpoint = 0.0; 104 | double PIDRollInput, PIDRollOutput; 105 | double PIDPitchInput, PIDPitchOutput; 106 | double kp = 1.5; 107 | double ki = 3.0; 108 | double kd = 0.001; 109 | double pid_out_min = -25.0; 110 | double pid_out_max = 25.0; 111 | PID bodyRollPID(&PIDRollInput, &PIDRollOutput, &Setpoint, kp, ki, kd, DIRECT); 112 | PID bodyPitchPID(&PIDPitchInput, &PIDPitchOutput, &Setpoint, kp, ki, kd, DIRECT); 113 | 114 | //////////////////////////// 115 | /// PWM Global variables /// 116 | //////////////////////////// 117 | float smooth_gain = 0.65; 118 | 119 | int pwm1_1; 120 | int pwm2_1; 121 | int pwm3_1; 122 | 123 | int pwm1_2; 124 | int pwm2_2; 125 | int pwm3_2; 126 | 127 | int pwm1_3; 128 | int pwm2_3; 129 | int pwm3_3; 130 | 131 | int pwm1_4; 132 | int pwm2_4; 133 | int pwm3_4; 134 | 135 | int pwm1_5; 136 | int pwm2_5; 137 | int pwm3_5; 138 | 139 | int pwm1_6; 140 | int pwm2_6; 141 | int pwm3_6; 142 | 143 | int pwm1_1_prev; 144 | int pwm2_1_prev; 145 | int pwm3_1_prev; 146 | 147 | int pwm1_2_prev; 148 | int pwm2_2_prev; 149 | int pwm3_2_prev; 150 | 151 | int pwm1_3_prev; 152 | int pwm2_3_prev; 153 | int pwm3_3_prev; 154 | 155 | int pwm1_4_prev; 156 | int pwm2_4_prev; 157 | int pwm3_4_prev; 158 | 159 | int pwm1_5_prev; 160 | int pwm2_5_prev; 161 | int pwm3_5_prev; 162 | 163 | int pwm1_6_prev; 164 | int pwm2_6_prev; 165 | int pwm3_6_prev; 166 | 167 | 168 | void setup() { 169 | 170 | Serial.begin(115200); 171 | 172 | /// BNO055 setup// 173 | if (!bno.begin()) 174 | { 175 | /* There was a problem detecting the BNO055 ... check your connections */ 176 | Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); 177 | while (1); 178 | } 179 | delay(1000); 180 | bno.setExtCrystalUse(true); 181 | 182 | /// PID /// 183 | bodyRollPID.SetMode(AUTOMATIC); 184 | bodyRollPID.SetOutputLimits(pid_out_min, pid_out_max); 185 | bodyPitchPID.SetMode(AUTOMATIC); 186 | bodyPitchPID.SetOutputLimits(pid_out_min, pid_out_max); 187 | 188 | /// SBUS parser thread /// 189 | uart_config_t uart2_config = { 190 | .baud_rate = 115200, 191 | .data_bits = UART_DATA_8_BITS, 192 | .parity = UART_PARITY_DISABLE, 193 | .stop_bits = UART_STOP_BITS_1, 194 | .flow_ctrl = UART_HW_FLOWCTRL_DISABLE 195 | }; 196 | uart_param_config(UART_PORT_NUM, &uart2_config); 197 | esp_log_level_set(TAG, ESP_LOG_INFO); 198 | uart_set_pin(UART_PORT_NUM, U2TXD, U2RXD, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); 199 | uart_driver_install(UART_PORT_NUM, BUF_SIZE, BUF_SIZE, 20, &uart2_queue, 0); 200 | xTaskCreate(UART_ISR_ROUTINE, "UART_ISR_ROUTINE", 2048, NULL, 12, NULL); 201 | 202 | /// PCA9685 setup /// 203 | pwm.begin(); 204 | pwm.setOscillatorFrequency(27000000); 205 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 206 | delay(5); 207 | 208 | pwm_2.begin(); 209 | pwm_2.setOscillatorFrequency(27000000); 210 | pwm_2.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 211 | delay(5); 212 | 213 | /// Hexapod setup /// 214 | h.generate_crabWalkingLUT(); 215 | h.generate_inplaceTurning_LUT(); 216 | h.generate_normalWalking_LUT(); 217 | 218 | /// Init Home motion /// 219 | init_global_PWM(); 220 | driveServo(); 221 | last_stamp = millis(); 222 | last_home_stamp = millis(); 223 | delay(1000); 224 | } 225 | 226 | void loop() { 227 | 228 | // copy sbus values from "ch" to "sbus_ch" 229 | memcpy(sbus_ch, ch, sizeof(ch)); 230 | 231 | // get data from BNO055 // 232 | sensors_event_t event; 233 | bno.getEvent(&event); 234 | 235 | imu::Quaternion quat = bno.getQuat(); 236 | qw = quat.w(); 237 | qx = quat.x(); 238 | qy = quat.y(); 239 | qz = quat.z(); 240 | r11 = pow(qw,2) + pow(qx,2) - pow(qy,2) - pow(qz,2); 241 | r12 = 2*qx*qy - 2*qz*qw; 242 | r13 = 2*qx*qz + 2*qy*qw; 243 | r21 = 2*qx*qy + 2*qz*qw; 244 | r22 = pow(qw,2) - pow(qx,2) + pow(qy,2) - pow(qz,2); 245 | r23 = 2*qy*qz - 2*qx*qw; 246 | r31 = 2*qx*qz - 2*qy*qw; 247 | r32 = 2*qy*qz + 2*qx*qw; 248 | r33 = pow(qw,2) - pow(qx,2) - pow(qy,2) + pow(qz,2); 249 | 250 | imu_roll = atan2(r32, r33) * RAD2DEG; 251 | imu_pitch = asin(-r31) * RAD2DEG; 252 | body_yaw = atan2(-r21, r11) * RAD2DEG; 253 | 254 | // we're using 5 channels 255 | ch1 = sbus_ch[0]; 256 | ch2 = sbus_ch[1]; 257 | ch4 = sbus_ch[3]; 258 | ch5 = sbus_ch[4]; 259 | ch7 = sbus_ch[6]; 260 | ch8 = sbus_ch[7]; 261 | 262 | // ch2 is mostly used for throttle speed 263 | // the more it's pushed, the smaller the delay between movement in LUT 264 | if (ch2 > sbus_db_max){ 265 | delay_time = map(ch2, sbus_mid, sbus_max, max_delay, min_delay); 266 | } else if (ch2 < sbus_db_min){ 267 | delay_time = map(ch2, sbus_mid, sbus_min, max_delay, min_delay); 268 | } else { 269 | delay_time = default_delay; 270 | } 271 | 272 | // just to check if sticks got pushed or not 273 | stick_ch1_pressed = ((ch1 > sbus_db_max) || (ch1 < sbus_db_min)); 274 | stick_ch2_pressed = ((ch2 > sbus_db_max) || (ch2 < sbus_db_min)); 275 | stick_ch4_pressed = ((ch4 > sbus_db_max) || (ch4 < sbus_db_min)); 276 | stick_ch5_pressed = ch5 < 1500; 277 | 278 | sticks_pressed = (stick_ch1_pressed || stick_ch2_pressed || stick_ch4_pressed || stick_ch5_pressed); 279 | 280 | ////////////////////// 281 | /// Normal walking /// 282 | ////////////////////// 283 | if (ch7 > 1500){ 284 | robot_mode_index = 0; 285 | 286 | // steering only front // 287 | if (ch2 > sbus_db_max) { 288 | if ((ch1 < sbus_db_max) && (ch1 > 1000)) { 289 | str_index = 7; 290 | } else if (ch1 >= sbus_db_max) { 291 | str_index = (int)map(ch1, sbus_mid, sbus_max, 7, 14); 292 | } else if (ch1 <= sbus_db_min) { 293 | str_index = (int)map(ch1, sbus_min, sbus_mid, 0, 7); 294 | } 295 | } else { 296 | str_index = 7; 297 | } 298 | } 299 | 300 | ////////////////////////////////////////// 301 | /// Crab-walking with in-place turning /// 302 | ////////////////////////////////////////// 303 | else if (ch7 > 1000){ 304 | robot_mode_index = 1; 305 | 306 | // crab-walking front // 307 | if (ch2 > sbus_db_max){ 308 | if (ch1 > sbus_db_max){ 309 | ang_inc = (int)map(ch1, sbus_db_max, sbus_max, 11, 6); 310 | } else if (ch1 < sbus_db_min){ 311 | ang_inc = (int)map(ch1, sbus_min, sbus_db_min, 6, 0); 312 | } else { 313 | ang_inc = 0; 314 | } 315 | } 316 | // crab-walking back // 317 | else if (ch2 < sbus_db_min){ 318 | if (ch1 > sbus_db_max){ 319 | ang_inc = (int)map(ch1, sbus_db_max, sbus_max, 6, 11); 320 | } else if (ch1 < sbus_db_min){ 321 | ang_inc = (int)map(ch1, sbus_min, sbus_db_min, 0, 6); 322 | } else { 323 | ang_inc = 6; 324 | } 325 | } 326 | // stay still // 327 | else { 328 | ang_inc = 0; 329 | } 330 | 331 | if (ch4 > sbus_db_max){ 332 | turn_inc = 0; 333 | } else { 334 | turn_inc = 1; 335 | } 336 | 337 | } 338 | 339 | ////////////////////////////////////// 340 | /// Rotation & Translation control /// 341 | ////////////////////////////////////// 342 | else { 343 | robot_mode_index = 2; 344 | 345 | // translation // 346 | if (ch8 > 1500){ 347 | x_trans = (float)map(ch4, sbus_min, sbus_max, -80.0, 80.0); 348 | y_trans = (float)map(ch2, sbus_min, sbus_max, -80.0, 80.0); 349 | z_trans = (float)map(ch1, sbus_min, sbus_max, -50.0, 50.0); 350 | } 351 | // rotation // 352 | else { 353 | roll = (float)map(ch4, sbus_min, sbus_max, -15.0, 15.0); 354 | pitch = (float)map(ch2, sbus_min, sbus_max, -15.0, 15.0); 355 | yaw = (float)map(ch1, sbus_min, sbus_max, -15.0, 15.0); 356 | } 357 | 358 | 359 | 360 | } 361 | 362 | //////////////////// 363 | /// LUT counter /// 364 | ////////////////// 365 | period = millis() - last_stamp; 366 | if ((period >= delay_time) && (sticks_pressed)) { 367 | 368 | if (start_drive){ 369 | walk_counter = (int)DATA_POINT_ALL/4; 370 | start_drive = false; 371 | } 372 | 373 | ///////////////////////////////// 374 | // normal-walking LUT counting // 375 | ///////////////////////////////// 376 | if (robot_mode_index == 0){ 377 | 378 | if (walk_counter < DATA_POINT_ALL){ 379 | setNormalWalkingPWM(str_index, walk_counter); 380 | walk_counter++; 381 | } else { 382 | walk_counter = 0; 383 | } 384 | 385 | smooth_gain = 0.85; 386 | 387 | } 388 | ///////////////////////////////////////// 389 | // crab-walking & turning LUT counting // 390 | ///////////////////////////////////////// 391 | else if (robot_mode_index == 1){ 392 | 393 | // crab-walking // 394 | if (stick_ch2_pressed){ 395 | if (walk_counter < DATA_POINT_ALL){ 396 | setCrabWalkingPWM(ang_inc, walk_counter); 397 | walk_counter++; 398 | } else { 399 | walk_counter = 0; 400 | } 401 | } 402 | // turning // 403 | else if (stick_ch4_pressed){ 404 | if (turn_counter < DATA_POINT_TURN_ALL){ 405 | setInPlaceTurningPWM(turn_inc, turn_counter); 406 | turn_counter++; 407 | } else { 408 | turn_counter = 0; 409 | } 410 | } 411 | 412 | smooth_gain = 0.65; 413 | 414 | } 415 | ///////////////////////////////// 416 | // body trans-rot LUT counting // 417 | ///////////////////////////////// 418 | else if (robot_mode_index == 2){ 419 | 420 | if (ch8 > 1500){ 421 | XYZ_to_PWM(x_trans, y_trans, z_trans); 422 | } else { 423 | if (ch5 > 1500){ 424 | RPY_to_PWM(roll, pitch, yaw); 425 | } else{ 426 | //comp_roll = -constrain(imu_pitch, -15.0, 15.0); 427 | //comp_pitch = constrain(imu_roll, -15.0, 15.0); 428 | PIDRollInput = -imu_pitch; 429 | PIDPitchInput = imu_roll; 430 | bodyRollPID.Compute(); 431 | bodyPitchPID.Compute(); 432 | 433 | // RPY_to_PWM(PIDRollOutput, 0.0, 0.0); 434 | RPY_to_PWM(PIDRollOutput, PIDPitchOutput, 0.0); 435 | } 436 | 437 | } 438 | } 439 | 440 | 441 | // if ((millis() - last_home_stamp) < 1500){ 442 | // do_smooth = true; 443 | // } else { 444 | // do_smooth = false; 445 | // } 446 | 447 | do_smooth = true; 448 | from_walk = true; 449 | last_stamp = millis(); 450 | 451 | } else if (sticks_pressed == false) { 452 | 453 | setHomePWM(); 454 | from_walk = false; 455 | walk_counter = 0; 456 | turn_counter = 0; 457 | do_smooth = true; 458 | start_drive = true; 459 | last_home_stamp = millis(); 460 | 461 | 462 | } else { 463 | 464 | // this is just a wait during next counter // 465 | // do nothing here // 466 | 467 | } 468 | 469 | //////////////////// 470 | /// Servo driving // 471 | //////////////////// 472 | if (do_smooth == true) { 473 | 474 | pwm1_1 = smoothingPWM(smooth_gain, pwm1_1, pwm1_1_prev); 475 | pwm2_1 = smoothingPWM(smooth_gain, pwm2_1, pwm2_1_prev); 476 | pwm3_1 = smoothingPWM(smooth_gain, pwm3_1, pwm3_1_prev); 477 | 478 | pwm1_2 = smoothingPWM(smooth_gain, pwm1_2, pwm1_2_prev); 479 | pwm2_2 = smoothingPWM(smooth_gain, pwm2_2, pwm2_2_prev); 480 | pwm3_2 = smoothingPWM(smooth_gain, pwm3_2, pwm3_2_prev); 481 | 482 | pwm1_3 = smoothingPWM(smooth_gain, pwm1_3, pwm1_3_prev); 483 | pwm2_3 = smoothingPWM(smooth_gain, pwm2_3, pwm2_3_prev); 484 | pwm3_3 = smoothingPWM(smooth_gain, pwm3_3, pwm3_3_prev); 485 | 486 | pwm1_4 = smoothingPWM(smooth_gain, pwm1_4, pwm1_4_prev); 487 | pwm2_4 = smoothingPWM(smooth_gain, pwm2_4, pwm2_4_prev); 488 | pwm3_4 = smoothingPWM(smooth_gain, pwm3_4, pwm3_4_prev); 489 | 490 | pwm1_5 = smoothingPWM(smooth_gain, pwm1_5, pwm1_5_prev); 491 | pwm2_5 = smoothingPWM(smooth_gain, pwm2_5, pwm2_5_prev); 492 | pwm3_5 = smoothingPWM(smooth_gain, pwm3_5, pwm3_5_prev); 493 | 494 | pwm1_6 = smoothingPWM(smooth_gain, pwm1_6, pwm1_6_prev); 495 | pwm2_6 = smoothingPWM(smooth_gain, pwm2_6, pwm2_6_prev); 496 | pwm3_6 = smoothingPWM(smooth_gain, pwm3_6, pwm3_6_prev); 497 | } 498 | 499 | pwm1_1_prev = pwm1_1; 500 | pwm2_1_prev = pwm2_1; 501 | pwm3_1_prev = pwm3_1; 502 | 503 | pwm1_2_prev = pwm1_2; 504 | pwm2_2_prev = pwm2_2; 505 | pwm3_2_prev = pwm3_2; 506 | 507 | pwm1_3_prev = pwm1_3; 508 | pwm2_3_prev = pwm2_3; 509 | pwm3_3_prev = pwm3_3; 510 | 511 | pwm1_4_prev = pwm1_4; 512 | pwm2_4_prev = pwm2_4; 513 | pwm3_4_prev = pwm3_4; 514 | 515 | pwm1_5_prev = pwm1_5; 516 | pwm2_5_prev = pwm2_5; 517 | pwm3_5_prev = pwm3_5; 518 | 519 | pwm1_6_prev = pwm1_6; 520 | pwm2_6_prev = pwm2_6; 521 | pwm3_6_prev = pwm3_6; 522 | 523 | driveServo(); 524 | 525 | /// Logging /// 526 | 527 | Serial.print("mode "); 528 | Serial.print(robot_mode[robot_mode_index]); 529 | // Serial.print(" ch1 "); 530 | // Serial.print(ch1); 531 | // Serial.print(" ch2 "); 532 | // Serial.print(ch2); 533 | // Serial.print(" ch4 "); 534 | // Serial.print(ch4); 535 | // Serial.print(" ch7 "); 536 | // Serial.print(ch7); 537 | // Serial.print(" ch5 "); 538 | // Serial.print(ch5); 539 | Serial.print(" smooth "); 540 | Serial.print(do_smooth); 541 | Serial.print(" smooth_K "); 542 | Serial.print(smooth_gain); 543 | Serial.print(" walk_counter "); 544 | Serial.print(walk_counter); 545 | Serial.print(" turn_counter "); 546 | Serial.print(turn_counter); 547 | Serial.print(" delay "); 548 | Serial.print(delay_time); 549 | Serial.print(" ang_in "); 550 | Serial.print(ang_inc); 551 | Serial.print(" turn_in "); 552 | Serial.print(turn_inc); 553 | Serial.print(" str_in "); 554 | Serial.print(str_index); 555 | // Serial.print(" r "); 556 | // Serial.print(roll); 557 | // Serial.print(" p "); 558 | // Serial.print(pitch); 559 | // Serial.print(" yw "); 560 | // Serial.print(yaw); 561 | // Serial.print(" imu_roll: "); 562 | // Serial.print(imu_roll, 2); 563 | // Serial.print(" imu_pitch: "); 564 | // Serial.print(imu_pitch, 2); 565 | // Serial.print(" pid_in_roll "); 566 | // Serial.print( PIDRollInput); 567 | // Serial.print(" pid_out_roll "); 568 | // Serial.print( PIDRollOutput); 569 | // Serial.print(" pid_in_pitch "); 570 | // Serial.print( PIDPitchInput); 571 | // Serial.print(" pid_out_pitch "); 572 | // Serial.println( PIDPitchOutput); 573 | // Serial.print(" body_yaw: "); 574 | // Serial.print(body_yaw, 2); 575 | // Serial.print(" comp_roll: "); 576 | // Serial.print(comp_roll, 2); 577 | // Serial.print(" comp_pitch: "); 578 | // Serial.println(comp_pitch, 2); 579 | // Serial.print(" x "); 580 | // Serial.print(x_trans); 581 | // Serial.print(" y "); 582 | // Serial.print(y_trans); 583 | // Serial.print(" z "); 584 | // Serial.print(z_trans); 585 | 586 | Serial.println(" "); 587 | 588 | 589 | // delay(1); 590 | } 591 | 592 | static void UART_ISR_ROUTINE(void *pvParameters) 593 | { 594 | uart_event_t event; 595 | size_t buffered_size; 596 | bool exit_condition = false; 597 | 598 | //Infinite loop to run main bulk of task 599 | while (1) { 600 | 601 | //Loop will continually block (i.e. wait) on event messages from the event queue 602 | if (xQueueReceive(uart2_queue, (void * )&event, (portTickType)portMAX_DELAY)) { 603 | 604 | //Handle received event 605 | if (event.type == UART_DATA) { 606 | 607 | uint8_t UART2_data[128]; 608 | //uint8_t buf[35]; 609 | //uint16_t ch[16]; 610 | int UART2_data_length = 0; 611 | ESP_ERROR_CHECK(uart_get_buffered_data_len(UART_NUM_2, (size_t*)&UART2_data_length)); 612 | UART2_data_length = uart_read_bytes(UART_NUM_2, UART2_data, UART2_data_length, 100); 613 | 614 | //Serial.println("LEN= ");Serial.println(UART2_data_length); 615 | 616 | //Serial.print("DATA= "); 617 | //for(byte i=0; i 2 | #include 3 | 4 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 5 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 6 | #define SERVO_FREQ 50 //50 // Analog servos run at ~50 Hz updates 7 | 8 | int input; 9 | 10 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); 11 | 12 | void setup() { 13 | 14 | Serial.begin(112500); 15 | pwm.begin(); 16 | 17 | pwm.setOscillatorFrequency(27000000); 18 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 19 | 20 | delay(10); 21 | 22 | } 23 | 24 | void loop() { 25 | 26 | if (Serial.available() > 0){ 27 | 28 | input = Serial.read(); 29 | 30 | Serial.println(input); 31 | 32 | } 33 | 34 | 35 | 36 | } 37 | -------------------------------------------------------------------------------- /test_codes/test_bno055/test_bno055.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #define RAD2DEG 180/PI 6 | Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire); 7 | 8 | unsigned long period; 9 | unsigned long last_stamp; 10 | float qw; 11 | float qx; 12 | float qy; 13 | float qz; 14 | float r11, r12, r13, r21, r22, r23, r31, r32, r33; 15 | 16 | float roll; 17 | float pitch; 18 | float yaw; 19 | 20 | void setup() { 21 | Serial.begin(115200); 22 | 23 | if (!bno.begin()) 24 | { 25 | /* There was a problem detecting the BNO055 ... check your connections */ 26 | Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); 27 | while (1); 28 | } 29 | 30 | delay(1000); 31 | bno.setExtCrystalUse(true); 32 | last_stamp = millis(); 33 | } 34 | 35 | void loop() { 36 | 37 | period = millis() - last_stamp; 38 | imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); 39 | 40 | sensors_event_t event; 41 | bno.getEvent(&event); 42 | 43 | imu::Quaternion quat = bno.getQuat(); 44 | qw = quat.w(); 45 | qx = quat.x(); 46 | qy = quat.y(); 47 | qz = quat.z(); 48 | r11 = pow(qw,2) + pow(qx,2) - pow(qy,2) - pow(qz,2); 49 | r12 = 2*qx*qy - 2*qz*qw; 50 | r13 = 2*qx*qz + 2*qy*qw; 51 | r21 = 2*qx*qy + 2*qz*qw; 52 | r22 = pow(qw,2) - pow(qx,2) + pow(qy,2) - pow(qz,2); 53 | r23 = 2*qy*qz - 2*qx*qw; 54 | r31 = 2*qx*qz - 2*qy*qw; 55 | r32 = 2*qy*qz + 2*qx*qw; 56 | r33 = pow(qw,2) - pow(qx,2) - pow(qy,2) + pow(qz,2); 57 | 58 | roll = atan2(r32, r33) * RAD2DEG; 59 | pitch = asin(-r31) * RAD2DEG; 60 | yaw = atan2(-r21, r11) * RAD2DEG; 61 | 62 | /* Display the floating point data */ 63 | Serial.print("period "); 64 | Serial.print(period, 6); 65 | Serial.print(" eul_X: "); 66 | Serial.print(euler.x()); 67 | Serial.print(" eul_Y: "); 68 | Serial.print(euler.y()); 69 | Serial.print(" eul_Z: "); 70 | Serial.print(euler.z()); 71 | /* Display the floating point data */ 72 | Serial.print(" ori_X: "); 73 | Serial.print(event.orientation.x, 4); 74 | Serial.print(" ori_Y: "); 75 | Serial.print(event.orientation.y, 4); 76 | Serial.print(" ori_Z: "); 77 | Serial.print(event.orientation.z, 4); 78 | Serial.print(" qW: "); 79 | Serial.print(quat.w(), 4); 80 | Serial.print(" qX: "); 81 | Serial.print(quat.y(), 4); 82 | Serial.print(" qY: "); 83 | Serial.print(quat.x(), 4); 84 | Serial.print(" qZ: "); 85 | Serial.print(quat.z(), 4); 86 | Serial.print(" roll: "); 87 | Serial.print(roll, 2); 88 | Serial.print(" pitch: "); 89 | Serial.print(pitch, 2); 90 | Serial.print(" yaw: "); 91 | Serial.print(yaw, 2); 92 | Serial.println(""); 93 | 94 | // delay(1); 95 | 96 | last_stamp = millis(); 97 | 98 | } 99 | -------------------------------------------------------------------------------- /test_codes/test_class/test_class.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 6 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 7 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 8 | 9 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); 10 | Adafruit_PWMServoDriver pwm_2 = Adafruit_PWMServoDriver(0x41); 11 | 12 | HexapodLib h; 13 | int counter = 0; 14 | 15 | void setup() { 16 | Serial.begin(115200); 17 | 18 | pwm.begin(); 19 | pwm.setOscillatorFrequency(27000000); 20 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 21 | delay(5); 22 | 23 | pwm_2.begin(); 24 | pwm_2.setOscillatorFrequency(27000000); 25 | pwm_2.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 26 | delay(5); 27 | 28 | // Serial.println("X1"); 29 | // print_array(h.x1, DATA_POINT_ALL); 30 | // Serial.println("Y1"); 31 | // print_array(h.y1, DATA_POINT_ALL); 32 | // Serial.println("Z1"); 33 | // print_array(h.z1, DATA_POINT_ALL); 34 | // 35 | // Serial.println("THETA1_1"); 36 | // print_array(h.the1_1, DATA_POINT_ALL); 37 | // Serial.println("THETA2_1"); 38 | // print_array(h.the2_1, DATA_POINT_ALL); 39 | // Serial.println("THETA3_1"); 40 | // print_array(h.the3_1, DATA_POINT_ALL); 41 | // 42 | // Serial.println("PWM1_1"); 43 | // print_array(h.pwm1_1, DATA_POINT_ALL); 44 | // Serial.println("PWM2_1"); 45 | // print_array(h.pwm2_1, DATA_POINT_ALL); 46 | // Serial.println("PWM3_1"); 47 | // print_array(h.pwm3_1, DATA_POINT_ALL); 48 | // Serial.println("******************************************************"); 49 | // 50 | // Serial.println("X2"); 51 | // print_array(h.X2, DATA_POINT_ALL); 52 | // Serial.println("Y2"); 53 | // print_array(h.Y2, DATA_POINT_ALL); 54 | // Serial.println("Z2"); 55 | // print_array(h.Z2, DATA_POINT_ALL); 56 | // 57 | // Serial.println("THETA1_2"); 58 | // print_array(h.THETA1_2, DATA_POINT_ALL); 59 | // Serial.println("THETA2_2"); 60 | // print_array(h.THETA2_2, DATA_POINT_ALL); 61 | // Serial.println("THETA3_2"); 62 | // print_array(h.THETA3_2, DATA_POINT_ALL); 63 | // 64 | // Serial.println("PWM1_2"); 65 | // print_array(h.PWM1_2, DATA_POINT_ALL); 66 | // Serial.println("PWM2_2"); 67 | // print_array(h.PWM2_2, DATA_POINT_ALL); 68 | // Serial.println("PWM3_2"); 69 | // print_array(h.PWM3_2, DATA_POINT_ALL); 70 | // Serial.println("******************************************************"); 71 | // 72 | // Serial.println("X6"); 73 | // print_array(h.X6, DATA_POINT_ALL); 74 | // Serial.println("Y6"); 75 | // print_array(h.Y6, DATA_POINT_ALL); 76 | // Serial.println("Z6"); 77 | // print_array(h.Z6, DATA_POINT_ALL); 78 | // 79 | // Serial.println("THETA1_6"); 80 | // print_array(h.THETA1_6, DATA_POINT_ALL); 81 | // Serial.println("THETA2_6"); 82 | // print_array(h.THETA2_6, DATA_POINT_ALL); 83 | // Serial.println("THETA3_6"); 84 | // print_array(h.THETA3_6, DATA_POINT_ALL); 85 | // 86 | // Serial.println("PWM1_6"); 87 | // print_array(h.PWM1_6, DATA_POINT_ALL); 88 | // Serial.println("PWM2_6"); 89 | // print_array(h.PWM2_6, DATA_POINT_ALL); 90 | // Serial.println("PWM3_6"); 91 | // print_array(h.PWM3_6, DATA_POINT_ALL); 92 | 93 | Serial.println("******************************************************"); 94 | Serial.print("PWM1_home "); 95 | Serial.println(h.PWM1_home); 96 | Serial.print("PWM2_home "); 97 | Serial.println(h.PWM2_home); 98 | Serial.print("PWM3_home "); 99 | Serial.println(h.PWM3_home); 100 | 101 | Serial.println("******************************************************"); 102 | // h.generate_crabWalkingLUT(); 103 | for (int i = 0; i < 12; i++) { 104 | Serial.print("Ang "); 105 | Serial.println(i*30); 106 | Serial.println("CW PWM1_1"); 107 | print_array(h.crab_walking_LUT[i][0], DATA_POINT_ALL); 108 | Serial.println("CW PWM2_1"); 109 | print_array(h.crab_walking_LUT[i][1], DATA_POINT_ALL); 110 | Serial.println("CW PWM3_1"); 111 | print_array(h.crab_walking_LUT[i][2], DATA_POINT_ALL); 112 | Serial.println("======================================"); 113 | Serial.println("CW PWM1_2"); 114 | print_array(h.crab_walking_LUT[i][3], DATA_POINT_ALL); 115 | Serial.println("CW PWM2_2"); 116 | print_array(h.crab_walking_LUT[i][4], DATA_POINT_ALL); 117 | Serial.println("CW PWM3_2"); 118 | print_array(h.crab_walking_LUT[i][5], DATA_POINT_ALL); 119 | Serial.println("======================================"); 120 | Serial.println("CW PWM1_3"); 121 | print_array(h.crab_walking_LUT[i][6], DATA_POINT_ALL); 122 | Serial.println("CW PWM2_3"); 123 | print_array(h.crab_walking_LUT[i][7], DATA_POINT_ALL); 124 | Serial.println("CW PWM3_3"); 125 | print_array(h.crab_walking_LUT[i][8], DATA_POINT_ALL); 126 | Serial.println("======================================"); 127 | Serial.println("CW PWM1_4"); 128 | print_array(h.crab_walking_LUT[i][9], DATA_POINT_ALL); 129 | Serial.println("CW PWM2_4"); 130 | print_array(h.crab_walking_LUT[i][10], DATA_POINT_ALL); 131 | Serial.println("CW PWM3_4"); 132 | print_array(h.crab_walking_LUT[i][11], DATA_POINT_ALL); 133 | Serial.println("======================================"); 134 | Serial.println("CW PWM1_5"); 135 | print_array(h.crab_walking_LUT[i][12], DATA_POINT_ALL); 136 | Serial.println("CW PWM2_5"); 137 | print_array(h.crab_walking_LUT[i][13], DATA_POINT_ALL); 138 | Serial.println("CW PWM3_5"); 139 | print_array(h.crab_walking_LUT[i][14], DATA_POINT_ALL); 140 | Serial.println("======================================"); 141 | Serial.println("CW PWM1_6"); 142 | print_array(h.crab_walking_LUT[i][15], DATA_POINT_ALL); 143 | Serial.println("CW PWM2_6"); 144 | print_array(h.crab_walking_LUT[i][16], DATA_POINT_ALL); 145 | Serial.println("CW PWM3_6"); 146 | print_array(h.crab_walking_LUT[i][17], DATA_POINT_ALL); 147 | 148 | Serial.println("******************************************************"); 149 | } 150 | 151 | // Serial.println("X3"); 152 | // print_array(h.X3, DATA_POINT_ALL); 153 | // Serial.println("Y3"); 154 | // print_array(h.Y3, DATA_POINT_ALL); 155 | // Serial.println("Z3"); 156 | // print_array(h.Z3, DATA_POINT_ALL); 157 | // 158 | // Serial.println("THETA1_3"); 159 | // print_array(h.THETA1_3, DATA_POINT_ALL); 160 | // Serial.println("THETA2_3"); 161 | // print_array(h.THETA2_3, DATA_POINT_ALL); 162 | // Serial.println("THETA3_3"); 163 | // print_array(h.THETA3_3, DATA_POINT_ALL); 164 | // 165 | // Serial.println("******************************************************"); 166 | 167 | 168 | 169 | 170 | 171 | pwm.writeMicroseconds(0, h.PWM1_home); 172 | pwm.writeMicroseconds(1, h.PWM2_home); 173 | pwm.writeMicroseconds(2, h.PWM3_home); 174 | 175 | pwm.writeMicroseconds(3, h.PWM1_home); 176 | pwm.writeMicroseconds(4, h.PWM2_home); 177 | pwm.writeMicroseconds(5, h.PWM3_home); 178 | 179 | pwm.writeMicroseconds(15, h.PWM1_home); 180 | pwm_2.writeMicroseconds(0, h.PWM2_home); 181 | pwm_2.writeMicroseconds(1, h.PWM3_home); 182 | 183 | delay(3000); 184 | } 185 | 186 | void loop() { 187 | // if (counter < DATA_POINT_ALL) { 188 | // 189 | // pwm.writeMicroseconds(0, h.PWM1_1[counter]); 190 | // pwm.writeMicroseconds(1, h.PWM2_1[counter]); 191 | // pwm.writeMicroseconds(2, h.PWM3_1[counter]); 192 | // 193 | // pwm.writeMicroseconds(3, h.PWM1_2[counter]); 194 | // pwm.writeMicroseconds(4, h.PWM2_2[counter]); 195 | // pwm.writeMicroseconds(5, h.PWM3_2[counter]); 196 | // 197 | // pwm.writeMicroseconds(15, h.PWM1_6[counter]); 198 | // pwm_2.writeMicroseconds(0, h.PWM2_6[counter]); 199 | // pwm_2.writeMicroseconds(1, h.PWM3_6[counter]); 200 | // 201 | // 202 | // counter++; 203 | // 204 | // } else { 205 | // 206 | // counter = 0; 207 | // } 208 | // 209 | // delay(10); 210 | 211 | } 212 | 213 | void print_array(double* arr, int len) { 214 | 215 | for (int i = 0; i < len; i++) { 216 | Serial.print(arr[i]); 217 | Serial.print(" "); 218 | } 219 | Serial.println(" "); 220 | 221 | } 222 | 223 | 224 | void print_array(float* arr, int len) { 225 | 226 | for (int i = 0; i < len; i++) { 227 | Serial.print(arr[i]); 228 | Serial.print(" "); 229 | } 230 | Serial.println(" "); 231 | 232 | } 233 | 234 | void print_array(int* arr, int len) { 235 | 236 | for (int i = 0; i < len; i++) { 237 | Serial.print(arr[i]); 238 | Serial.print(" "); 239 | } 240 | Serial.println(" "); 241 | 242 | } 243 | 244 | //void print_cw_LUT(double* arr) { 245 | // for (int i = 0; i < 12; i++) { 246 | // for (int j = 0; j < 18; j++) { 247 | // for (int k=0; k < 40; j++){ 248 | // 249 | // } 250 | // } 251 | // } 252 | //} 253 | -------------------------------------------------------------------------------- /test_codes/test_drive_servo/test_drive_servo.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 5 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 6 | #define SERVO_FREQ 50 //50 // Analog servos run at ~50 Hz updates 7 | 8 | 9 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); 10 | 11 | void setup() { 12 | 13 | Serial.begin(112500); 14 | pwm.begin(); 15 | 16 | pwm.setOscillatorFrequency(27000000); 17 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 18 | 19 | delay(10); 20 | 21 | // Futaba A700 22 | //HIGH 1900 23 | //MID 1350 24 | //LOW 800 25 | // pwm.writeMicroseconds(0, 1350); 26 | // pwm.writeMicroseconds(1, 1500); 27 | // pwm.writeMicroseconds(2, 1400); 28 | 29 | /// Futaba A700 CB700 30 | //double kin_deg = 0.0; 31 | //double servo_deg = map_with_limit(kin_deg, -90.0, 90.0, 0.0, 180.0); 32 | //Serial.print("servo_deg "); 33 | //Serial.println(servo_deg); 34 | //int pwm_val = (int)map_with_limit(servo_deg, 0.0, 180.0, 800.0, 1900.0); 35 | //Serial.print("pwm_val "); 36 | //Serial.println(pwm_val); 37 | 38 | /// Futaba DS3225 39 | double kin_deg = 0.0; 40 | double servo_deg = map_with_limit(kin_deg, -70.0, 70.0, 20.0, 160.0); 41 | Serial.print("servo_deg "); 42 | Serial.println(servo_deg); 43 | int pwm_val = (int)map_with_limit(servo_deg, 20.0, 160.0, 800.0, 2200.0); 44 | Serial.print("pwm_val "); 45 | Serial.println(pwm_val); 46 | 47 | pwm.writeMicroseconds(11, pwm_val); 48 | 49 | } 50 | 51 | void loop() { 52 | 53 | // pwm.writeMicroseconds(0, 800); 54 | // 55 | // Serial.println("LOW"); 56 | // 57 | // delay(2000); 58 | // 59 | // pwm.writeMicroseconds(0, 1350); 60 | // 61 | // Serial.println("MID"); 62 | // 63 | // delay(2000); 64 | // 65 | // pwm.writeMicroseconds(0, 1900); 66 | // 67 | // Serial.println("HIGH"); 68 | // 69 | // delay(2000); 70 | 71 | } 72 | 73 | double map_with_limit(double val, double in_min, double in_max, double out_min, double out_max) { 74 | 75 | double m; 76 | double out; 77 | 78 | m = (out_max - out_min) / (in_max - in_min); 79 | out = m * (val - in_min) + out_min; 80 | 81 | if (out_min > out_max) { 82 | if (out > out_min) { 83 | out = out_min; 84 | } 85 | else if (out < out_max) { 86 | out = out_max; 87 | } 88 | 89 | } 90 | else if (out_max > out_min) { 91 | if (out > out_max) { 92 | out = out_max; 93 | } 94 | else if (out < out_min) { 95 | out = out_min; 96 | } 97 | } 98 | 99 | return out; 100 | 101 | } 102 | -------------------------------------------------------------------------------- /test_codes/test_drive_servo_0x41/test_drive_servo_0x41.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 5 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 6 | #define SERVO_FREQ 50 //50 // Analog servos run at ~50 Hz updates 7 | 8 | 9 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41); 10 | 11 | void setup() { 12 | 13 | Serial.begin(112500); 14 | pwm.begin(); 15 | 16 | pwm.setOscillatorFrequency(27000000); 17 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 18 | 19 | delay(10); 20 | 21 | // Futaba A700 22 | //HIGH 1900 23 | //MID 1350 24 | //LOW 800 25 | // pwm.writeMicroseconds(0, 1350); 26 | // pwm.writeMicroseconds(1, 1500); 27 | // pwm.writeMicroseconds(2, 1400); 28 | 29 | /// Futaba A700 CB700 30 | //double kin_deg = 0.0; 31 | //double servo_deg = map_with_limit(kin_deg, -90.0, 90.0, 0.0, 180.0); 32 | //Serial.print("servo_deg "); 33 | //Serial.println(servo_deg); 34 | //int pwm_val = (int)map_with_limit(servo_deg, 0.0, 180.0, 800.0, 1900.0); 35 | //Serial.print("pwm_val "); 36 | //Serial.println(pwm_val); 37 | 38 | /// Futaba DS3225 39 | double kin_deg = 0.0; 40 | double servo_deg = map_with_limit(kin_deg, -70.0, 70.0, 20.0, 160.0); 41 | Serial.print("servo_deg "); 42 | Serial.println(servo_deg); 43 | int pwm_val = (int)map_with_limit(servo_deg, 20.0, 160.0, 800.0, 2200.0); 44 | Serial.print("pwm_val "); 45 | Serial.println(pwm_val); 46 | 47 | pwm.writeMicroseconds(0, pwm_val); 48 | pwm.writeMicroseconds(1, pwm_val); 49 | 50 | } 51 | 52 | void loop() { 53 | 54 | // pwm.writeMicroseconds(0, 800); 55 | // 56 | // Serial.println("LOW"); 57 | // 58 | // delay(2000); 59 | // 60 | // pwm.writeMicroseconds(0, 1350); 61 | // 62 | // Serial.println("MID"); 63 | // 64 | // delay(2000); 65 | // 66 | // pwm.writeMicroseconds(0, 1900); 67 | // 68 | // Serial.println("HIGH"); 69 | // 70 | // delay(2000); 71 | 72 | } 73 | 74 | double map_with_limit(double val, double in_min, double in_max, double out_min, double out_max) { 75 | 76 | double m; 77 | double out; 78 | 79 | m = (out_max - out_min) / (in_max - in_min); 80 | out = m * (val - in_min) + out_min; 81 | 82 | if (out_min > out_max) { 83 | if (out > out_min) { 84 | out = out_min; 85 | } 86 | else if (out < out_max) { 87 | out = out_max; 88 | } 89 | 90 | } 91 | else if (out_max > out_min) { 92 | if (out > out_max) { 93 | out = out_max; 94 | } 95 | else if (out < out_min) { 96 | out = out_min; 97 | } 98 | } 99 | 100 | return out; 101 | 102 | } 103 | -------------------------------------------------------------------------------- /test_codes/test_first_second_legs/test_first_second_legs.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 5 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 6 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 7 | 8 | #define L1 60 //40 9 | #define L2 80 10 | #define L3 171 //163 11 | 12 | #define RAD2DEG 180/PI 13 | #define DEG2RAD PI/180 14 | 15 | #define S -150 // starting point offset from 0 16 | #define T 100 // step distance (how far the foot will move) 17 | #define A 50 // step height (how the foot will lift from ground) 18 | #define DATA_POINT_PER_LINE 50 19 | #define DATA_POINT_ALL DATA_POINT_PER_LINE*2 20 | 21 | #define pwm1_min 800.0 22 | #define pwm1_mid 1350.0 23 | #define pwm1_max 1900.0 24 | 25 | #define pwm2_min 800.0 26 | #define pwm2_mid 1500.0 27 | #define pwm2_max 2200.0 28 | 29 | #define pwm3_min 800.0 30 | #define pwm3_mid 1500.0 31 | #define pwm3_max 2200.0 32 | 33 | 34 | double x = 150.0; 35 | double y = 100.0; 36 | double z = -100.0; 37 | 38 | double x_start = 160.0; 39 | 40 | double theta1; 41 | double theta2; 42 | double theta3; 43 | 44 | double X1_line[DATA_POINT_PER_LINE]; 45 | double Y1_line[DATA_POINT_PER_LINE]; 46 | double Z1_line[DATA_POINT_PER_LINE]; 47 | 48 | double X2_line[DATA_POINT_PER_LINE]; 49 | double Y2_line[DATA_POINT_PER_LINE]; 50 | double Z2_line[DATA_POINT_PER_LINE]; 51 | 52 | double X1_curve[DATA_POINT_PER_LINE]; 53 | double Y1_curve[DATA_POINT_PER_LINE]; 54 | double Z1_curve[DATA_POINT_PER_LINE]; 55 | 56 | double X2_curve[DATA_POINT_PER_LINE]; 57 | double Y2_curve[DATA_POINT_PER_LINE]; 58 | double Z2_curve[DATA_POINT_PER_LINE]; 59 | 60 | double X1[DATA_POINT_ALL]; 61 | double Y1[DATA_POINT_ALL]; 62 | double Z1[DATA_POINT_ALL]; 63 | 64 | double X2[DATA_POINT_ALL]; 65 | double Y2[DATA_POINT_ALL]; 66 | double Z2[DATA_POINT_ALL]; 67 | 68 | double P1[2] = { -T / 2, S}; 69 | double P2[2] = {0, S + (2 * A)}; 70 | double P3[2] = {T / 2, S}; 71 | double t[DATA_POINT_PER_LINE]; 72 | 73 | double THETA1_1[DATA_POINT_ALL]; 74 | double THETA2_1[DATA_POINT_ALL]; 75 | double THETA3_1[DATA_POINT_ALL]; 76 | 77 | double THETA1_2[DATA_POINT_ALL]; 78 | double THETA2_2[DATA_POINT_ALL]; 79 | double THETA3_2[DATA_POINT_ALL]; 80 | 81 | int PWM1_1[DATA_POINT_ALL]; 82 | int PWM2_1[DATA_POINT_ALL]; 83 | int PWM3_1[DATA_POINT_ALL]; 84 | 85 | int PWM1_2[DATA_POINT_ALL]; 86 | int PWM2_2[DATA_POINT_ALL]; 87 | int PWM3_2[DATA_POINT_ALL]; 88 | 89 | double beta = 55.71; 90 | double rot2_ang = -beta * DEG2RAD; 91 | double R11_2 = cos(rot2_ang); 92 | double R12_2 = -sin(rot2_ang); 93 | double R13_2 = 0.0; 94 | double R21_2 = sin(rot2_ang); 95 | double R22_2 = cos(rot2_ang); 96 | double R23_2 = 0.0; 97 | double R31_2 = 0.0; 98 | double R32_2 = 0.0; 99 | double R33_2 = 1.0; 100 | 101 | int counter = 0; 102 | 103 | // sizeof() returns number of bytes not array's length 104 | 105 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); 106 | 107 | void setup() { 108 | 109 | Serial.begin(115200); 110 | pwm.begin(); 111 | 112 | pwm.setOscillatorFrequency(27000000); 113 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 114 | 115 | delay(5); 116 | 117 | ///////////////////// 118 | /// Leg 1 (leg i) /// 119 | ///////////////////// 120 | linspace(x_start, x_start, DATA_POINT_PER_LINE, X1_line); 121 | linspace(S, S, DATA_POINT_PER_LINE, Z1_line); 122 | 123 | linspace(T / 2, -T / 2, DATA_POINT_PER_LINE, Y1_line); 124 | 125 | // Create Bezier curve 126 | linspace(0.0, 1.0, DATA_POINT_PER_LINE, t); 127 | linspace(x_start, x_start, DATA_POINT_PER_LINE, X1_curve); 128 | for (int i = 0; i < DATA_POINT_PER_LINE; i++) { 129 | Y1_curve[i] = (pow((1 - t[i]), 2) * P1[0]) + (2 * (1 - t[i]) * t[i] * P2[0]) + (pow(t[i], 2) * P3[0]); 130 | Z1_curve[i] = (pow((1 - t[i]), 2) * P1[1]) + (2 * (1 - t[i]) * t[i] * P2[1]) + (pow(t[i], 2) * P3[1]); 131 | } 132 | concat_arrays(X1_curve, X1_line, DATA_POINT_PER_LINE, DATA_POINT_PER_LINE, X1); 133 | concat_arrays(Y1_curve, Y1_line, DATA_POINT_PER_LINE, DATA_POINT_PER_LINE, Y1); 134 | concat_arrays(Z1_curve, Z1_line, DATA_POINT_PER_LINE, DATA_POINT_PER_LINE, Z1); 135 | 136 | inv_arrays(X1, Y1, Z1, DATA_POINT_ALL, THETA1_1, THETA2_1, THETA3_1); 137 | 138 | kinAngleArray_To_servoPwmArray(0, THETA1_1, DATA_POINT_ALL, PWM1_1); 139 | kinAngleArray_To_servoPwmArray(1, THETA2_1, DATA_POINT_ALL, PWM2_1); 140 | kinAngleArray_To_servoPwmArray(2, THETA3_1, DATA_POINT_ALL, PWM3_1); 141 | 142 | Serial.println("X1"); 143 | print_array(X1, DATA_POINT_ALL); 144 | Serial.println("Y1"); 145 | print_array(Y1, DATA_POINT_ALL); 146 | Serial.println("Z1"); 147 | print_array(Z1, DATA_POINT_ALL); 148 | 149 | Serial.println("THETA1_1"); 150 | print_array(THETA1_1, DATA_POINT_ALL); 151 | Serial.println("THETA2_1"); 152 | print_array(THETA2_1, DATA_POINT_ALL); 153 | Serial.println("THETA3_1"); 154 | print_array(THETA3_1, DATA_POINT_ALL); 155 | 156 | Serial.println("PWM1_1"); 157 | print_array(PWM1_1, DATA_POINT_ALL); 158 | Serial.println("PWM2_1"); 159 | print_array(PWM2_1, DATA_POINT_ALL); 160 | Serial.println("PWM3_1"); 161 | print_array(PWM3_1, DATA_POINT_ALL); 162 | Serial.println("******************************************************"); 163 | //////////////////// 164 | /// Leg2 (leg j) /// 165 | /////////////////// 166 | double start_time = millis(); 167 | linspace(0.0, 0.0, DATA_POINT_PER_LINE, X2_line); 168 | linspace(S, S, DATA_POINT_PER_LINE, Z2_line); 169 | 170 | linspace(T / 2, -T / 2, DATA_POINT_PER_LINE, Y2_line); 171 | 172 | // Create Bezier curve 173 | linspace(0.0, 0.0, DATA_POINT_PER_LINE, X2_curve); 174 | for (int i = 0; i < DATA_POINT_PER_LINE; i++) { 175 | Y2_curve[i] = (pow((1 - t[i]), 2) * P1[0]) + (2 * (1 - t[i]) * t[i] * P2[0]) + (pow(t[i], 2) * P3[0]); 176 | Z2_curve[i] = (pow((1 - t[i]), 2) * P1[1]) + (2 * (1 - t[i]) * t[i] * P2[1]) + (pow(t[i], 2) * P3[1]); 177 | } 178 | 179 | 180 | for (int i = 0; i out_max) { 354 | if (out > out_min) { 355 | out = out_min; 356 | } 357 | else if (out < out_max) { 358 | out = out_max; 359 | } 360 | 361 | } 362 | else if (out_max > out_min) { 363 | if (out > out_max) { 364 | out = out_max; 365 | } 366 | else if (out < out_min) { 367 | out = out_min; 368 | } 369 | } 370 | 371 | return out; 372 | 373 | } 374 | 375 | 376 | void kinAngleArray_To_servoPwmArray(int servo_id, double* THETA, int len, int* PWM) { 377 | 378 | double pwm_val; 379 | 380 | for (int i = 0; i < len; i++) { 381 | // if (servo_id == 0 || servo_id == 1) { 382 | // pwm_val = map_with_limit(THETA[i], -70.0, 70.0, 800.0, 2200.0); 383 | // } else { 384 | // pwm_val = map_with_limit(THETA[i], -160.0, -20.0, 2200.0, 800.0); 385 | // } 386 | 387 | if (servo_id == 0) { 388 | pwm_val = map_with_limit(THETA[i], -90.0, 90.0, pwm1_min, pwm1_max); 389 | } else if (servo_id == 1) { 390 | pwm_val = map_with_limit(THETA[i], -70.0, 70.0, pwm2_min, pwm2_max); 391 | } else { 392 | pwm_val = map_with_limit(THETA[i], -160.0, -20.0, pwm3_min, pwm3_max); 393 | } 394 | 395 | PWM[i] = int(pwm_val); 396 | 397 | } 398 | 399 | } 400 | -------------------------------------------------------------------------------- /test_codes/test_ijn_legs/test_ijn_legs.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 5 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 6 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 7 | 8 | #define L1 60 //40 9 | #define L2 80 10 | #define L3 171 //163 11 | 12 | #define RAD2DEG 180/PI 13 | #define DEG2RAD PI/180 14 | 15 | #define S -150 // starting point offset from 0 16 | #define T 100 // step distance (how far the foot will move) 17 | #define A 50 // step height (how the foot will lift from ground) 18 | #define DATA_POINT_PER_LINE 50 19 | #define DATA_POINT_ALL DATA_POINT_PER_LINE*2 20 | 21 | #define pwm1_min 800.0 22 | #define pwm1_mid 1350.0 23 | #define pwm1_max 1900.0 24 | 25 | #define pwm2_min 800.0 26 | #define pwm2_mid 1500.0 27 | #define pwm2_max 2200.0 28 | 29 | #define pwm3_min 800.0 30 | #define pwm3_mid 1500.0 31 | #define pwm3_max 2200.0 32 | 33 | 34 | double x = 150.0; 35 | double y = 100.0; 36 | double z = -100.0; 37 | 38 | double x_start = 160.0; 39 | 40 | double theta1; 41 | double theta2; 42 | double theta3; 43 | 44 | double X1_line[DATA_POINT_PER_LINE]; 45 | double Y1_line[DATA_POINT_PER_LINE]; 46 | double Z1_line[DATA_POINT_PER_LINE]; 47 | 48 | double X2_line[DATA_POINT_PER_LINE]; 49 | double Y2_line[DATA_POINT_PER_LINE]; 50 | double Z2_line[DATA_POINT_PER_LINE]; 51 | 52 | double X6_line[DATA_POINT_PER_LINE]; 53 | double Y6_line[DATA_POINT_PER_LINE]; 54 | double Z6_line[DATA_POINT_PER_LINE]; 55 | 56 | double X1_curve[DATA_POINT_PER_LINE]; 57 | double Y1_curve[DATA_POINT_PER_LINE]; 58 | double Z1_curve[DATA_POINT_PER_LINE]; 59 | 60 | double X2_curve[DATA_POINT_PER_LINE]; 61 | double Y2_curve[DATA_POINT_PER_LINE]; 62 | double Z2_curve[DATA_POINT_PER_LINE]; 63 | 64 | double X6_curve[DATA_POINT_PER_LINE]; 65 | double Y6_curve[DATA_POINT_PER_LINE]; 66 | double Z6_curve[DATA_POINT_PER_LINE]; 67 | 68 | double X1[DATA_POINT_ALL]; 69 | double Y1[DATA_POINT_ALL]; 70 | double Z1[DATA_POINT_ALL]; 71 | 72 | double X2[DATA_POINT_ALL]; 73 | double Y2[DATA_POINT_ALL]; 74 | double Z2[DATA_POINT_ALL]; 75 | 76 | double X6[DATA_POINT_ALL]; 77 | double Y6[DATA_POINT_ALL]; 78 | double Z6[DATA_POINT_ALL]; 79 | 80 | double P1[2] = { -T / 2, S}; 81 | double P2[2] = {0, S + (2 * A)}; 82 | double P3[2] = {T / 2, S}; 83 | double t[DATA_POINT_PER_LINE]; 84 | 85 | double THETA1_1[DATA_POINT_ALL]; 86 | double THETA2_1[DATA_POINT_ALL]; 87 | double THETA3_1[DATA_POINT_ALL]; 88 | 89 | double THETA1_2[DATA_POINT_ALL]; 90 | double THETA2_2[DATA_POINT_ALL]; 91 | double THETA3_2[DATA_POINT_ALL]; 92 | 93 | double THETA1_6[DATA_POINT_ALL]; 94 | double THETA2_6[DATA_POINT_ALL]; 95 | double THETA3_6[DATA_POINT_ALL]; 96 | 97 | int PWM1_1[DATA_POINT_ALL]; 98 | int PWM2_1[DATA_POINT_ALL]; 99 | int PWM3_1[DATA_POINT_ALL]; 100 | 101 | int PWM1_2[DATA_POINT_ALL]; 102 | int PWM2_2[DATA_POINT_ALL]; 103 | int PWM3_2[DATA_POINT_ALL]; 104 | 105 | int PWM1_6[DATA_POINT_ALL]; 106 | int PWM2_6[DATA_POINT_ALL]; 107 | int PWM3_6[DATA_POINT_ALL]; 108 | 109 | double beta = 55.71; 110 | double rot2_ang = -beta * DEG2RAD; 111 | double rot6_ang = beta * DEG2RAD; 112 | 113 | double R11_2 = cos(rot2_ang); 114 | double R12_2 = -sin(rot2_ang); 115 | double R13_2 = 0.0; 116 | double R21_2 = sin(rot2_ang); 117 | double R22_2 = cos(rot2_ang); 118 | double R23_2 = 0.0; 119 | double R31_2 = 0.0; 120 | double R32_2 = 0.0; 121 | double R33_2 = 1.0; 122 | 123 | double R11_6 = cos(rot6_ang); 124 | double R12_6 = -sin(rot6_ang); 125 | double R13_6 = 0.0; 126 | double R21_6 = sin(rot6_ang); 127 | double R22_6 = cos(rot6_ang); 128 | double R23_6 = 0.0; 129 | double R31_6 = 0.0; 130 | double R32_6 = 0.0; 131 | double R33_6 = 1.0; 132 | 133 | int counter = 0; 134 | int j; 135 | 136 | // sizeof() returns number of bytes not array's length 137 | 138 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); 139 | Adafruit_PWMServoDriver pwm_2 = Adafruit_PWMServoDriver(0x41); 140 | 141 | void setup() { 142 | 143 | Serial.begin(115200); 144 | 145 | pwm.begin(); 146 | pwm.setOscillatorFrequency(27000000); 147 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 148 | delay(5); 149 | 150 | pwm_2.begin(); 151 | pwm_2.setOscillatorFrequency(27000000); 152 | pwm_2.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 153 | delay(5); 154 | 155 | ///////////////////// 156 | /// Leg 1 (leg i) /// 157 | ///////////////////// 158 | linspace(x_start, x_start, DATA_POINT_PER_LINE, X1_line); 159 | linspace(S, S, DATA_POINT_PER_LINE, Z1_line); 160 | 161 | linspace(T / 2, -T / 2, DATA_POINT_PER_LINE, Y1_line); 162 | 163 | // Create Bezier curve 164 | linspace(0.0, 1.0, DATA_POINT_PER_LINE, t); 165 | linspace(x_start, x_start, DATA_POINT_PER_LINE, X1_curve); 166 | for (int i = 0; i < DATA_POINT_PER_LINE; i++) { 167 | Y1_curve[i] = (pow((1 - t[i]), 2) * P1[0]) + (2 * (1 - t[i]) * t[i] * P2[0]) + (pow(t[i], 2) * P3[0]); 168 | Z1_curve[i] = (pow((1 - t[i]), 2) * P1[1]) + (2 * (1 - t[i]) * t[i] * P2[1]) + (pow(t[i], 2) * P3[1]); 169 | } 170 | concat_arrays(X1_curve, X1_line, DATA_POINT_PER_LINE, DATA_POINT_PER_LINE, X1); 171 | concat_arrays(Y1_curve, Y1_line, DATA_POINT_PER_LINE, DATA_POINT_PER_LINE, Y1); 172 | concat_arrays(Z1_curve, Z1_line, DATA_POINT_PER_LINE, DATA_POINT_PER_LINE, Z1); 173 | 174 | inv_arrays(X1, Y1, Z1, DATA_POINT_ALL, THETA1_1, THETA2_1, THETA3_1); 175 | 176 | kinAngleArray_To_servoPwmArray(0, THETA1_1, DATA_POINT_ALL, PWM1_1); 177 | kinAngleArray_To_servoPwmArray(1, THETA2_1, DATA_POINT_ALL, PWM2_1); 178 | kinAngleArray_To_servoPwmArray(2, THETA3_1, DATA_POINT_ALL, PWM3_1); 179 | 180 | Serial.println("X1"); 181 | print_array(X1, DATA_POINT_ALL); 182 | Serial.println("Y1"); 183 | print_array(Y1, DATA_POINT_ALL); 184 | Serial.println("Z1"); 185 | print_array(Z1, DATA_POINT_ALL); 186 | 187 | Serial.println("THETA1_1"); 188 | print_array(THETA1_1, DATA_POINT_ALL); 189 | Serial.println("THETA2_1"); 190 | print_array(THETA2_1, DATA_POINT_ALL); 191 | Serial.println("THETA3_1"); 192 | print_array(THETA3_1, DATA_POINT_ALL); 193 | 194 | Serial.println("PWM1_1"); 195 | print_array(PWM1_1, DATA_POINT_ALL); 196 | Serial.println("PWM2_1"); 197 | print_array(PWM2_1, DATA_POINT_ALL); 198 | Serial.println("PWM3_1"); 199 | print_array(PWM3_1, DATA_POINT_ALL); 200 | Serial.println("******************************************************"); 201 | 202 | //////////////////// 203 | /// Leg2 (leg j) /// 204 | /////////////////// 205 | linspace(0.0, 0.0, DATA_POINT_PER_LINE, X2_line); 206 | linspace(S, S, DATA_POINT_PER_LINE, Z2_line); 207 | 208 | linspace(T / 2, -T / 2, DATA_POINT_PER_LINE, Y2_line); 209 | 210 | // Create Bezier curve 211 | linspace(0.0, 0.0, DATA_POINT_PER_LINE, X2_curve); 212 | for (int i = 0; i < DATA_POINT_PER_LINE; i++) { 213 | Y2_curve[i] = (pow((1 - t[i]), 2) * P1[0]) + (2 * (1 - t[i]) * t[i] * P2[0]) + (pow(t[i], 2) * P3[0]); 214 | Z2_curve[i] = (pow((1 - t[i]), 2) * P1[1]) + (2 * (1 - t[i]) * t[i] * P2[1]) + (pow(t[i], 2) * P3[1]); 215 | } 216 | 217 | 218 | for (int i = 0; i < DATA_POINT_PER_LINE; i++) { 219 | X2[i] = R11_2 * X2_line[i] + R12_2 * Y2_line[i] + R13_2 * Z2_line[i] + x_start; 220 | Y2[i] = R21_2 * X2_line[i] + R22_2 * Y2_line[i] + R23_2 * Z2_line[i]; 221 | Z2[i] = R31_2 * X2_line[i] + R32_2 * Y2_line[i] + R33_2 * Z2_line[i]; 222 | } 223 | 224 | j = 0; 225 | for (int i = DATA_POINT_PER_LINE; i < DATA_POINT_ALL; i++) { 226 | X2[i] = R11_2 * X2_curve[j] + R12_2 * Y2_curve[j] + R13_2 * Z2_curve[j] + x_start; 227 | Y2[i] = R21_2 * X2_curve[j] + R22_2 * Y2_curve[j] + R23_2 * Z2_curve[j]; 228 | Z2[i] = R31_2 * X2_curve[j] + R32_2 * Y2_curve[j] + R33_2 * Z2_curve[j]; 229 | j++; 230 | } 231 | 232 | inv_arrays(X2, Y2, Z2, DATA_POINT_ALL, THETA1_2, THETA2_2, THETA3_2); 233 | 234 | kinAngleArray_To_servoPwmArray(0, THETA1_2, DATA_POINT_ALL, PWM1_2); 235 | kinAngleArray_To_servoPwmArray(1, THETA2_2, DATA_POINT_ALL, PWM2_2); 236 | kinAngleArray_To_servoPwmArray(2, THETA3_2, DATA_POINT_ALL, PWM3_2); 237 | 238 | Serial.println("X2"); 239 | print_array(X2, DATA_POINT_ALL); 240 | Serial.println("Y2"); 241 | print_array(Y2, DATA_POINT_ALL); 242 | Serial.println("Z2"); 243 | print_array(Z2, DATA_POINT_ALL); 244 | 245 | Serial.println("THETA1_2"); 246 | print_array(THETA1_2, DATA_POINT_ALL); 247 | Serial.println("THETA2_2"); 248 | print_array(THETA2_2, DATA_POINT_ALL); 249 | Serial.println("THETA3_2"); 250 | print_array(THETA3_2, DATA_POINT_ALL); 251 | 252 | Serial.println("PWM1_2"); 253 | print_array(PWM1_2, DATA_POINT_ALL); 254 | Serial.println("PWM2_2"); 255 | print_array(PWM2_2, DATA_POINT_ALL); 256 | Serial.println("PWM3_2"); 257 | print_array(PWM3_2, DATA_POINT_ALL); 258 | Serial.println("******************************************************"); 259 | 260 | //////////////////// 261 | /// Leg6 (leg n) /// 262 | /////////////////// 263 | linspace(0.0, 0.0, DATA_POINT_PER_LINE, X6_line); 264 | linspace(S, S, DATA_POINT_PER_LINE, Z6_line); 265 | 266 | linspace(T / 2, -T / 2, DATA_POINT_PER_LINE, Y6_line); 267 | 268 | // Create Bezier curve 269 | linspace(0.0, 0.0, DATA_POINT_PER_LINE, X6_curve); 270 | for (int i = 0; i < DATA_POINT_PER_LINE; i++) { 271 | Y6_curve[i] = (pow((1 - t[i]), 2) * P1[0]) + (2 * (1 - t[i]) * t[i] * P2[0]) + (pow(t[i], 2) * P3[0]); 272 | Z6_curve[i] = (pow((1 - t[i]), 2) * P1[1]) + (2 * (1 - t[i]) * t[i] * P2[1]) + (pow(t[i], 2) * P3[1]); 273 | } 274 | 275 | for (int i = 0; i < DATA_POINT_PER_LINE; i++) { 276 | X6[i] = R11_6 * X6_line[i] + R12_6 * Y6_line[i] + R13_6 * Z6_line[i] + x_start; 277 | Y6[i] = R21_6 * X6_line[i] + R22_6 * Y6_line[i] + R23_6 * Z6_line[i]; 278 | Z6[i] = R31_6 * X6_line[i] + R32_6 * Y6_line[i] + R33_6 * Z6_line[i]; 279 | } 280 | 281 | j = 0; 282 | for (int i = DATA_POINT_PER_LINE; i < DATA_POINT_ALL; i++) { 283 | X6[i] = R11_6 * X6_curve[j] + R12_6 * Y6_curve[j] + R13_6 * Z6_curve[j] + x_start; 284 | Y6[i] = R21_6 * X6_curve[j] + R22_6 * Y6_curve[j] + R23_6 * Z6_curve[j]; 285 | Z6[i] = R31_6 * X6_curve[j] + R32_6 * Y6_curve[j] + R33_6 * Z6_curve[j]; 286 | j++; 287 | } 288 | 289 | inv_arrays(X6, Y6, Z6, DATA_POINT_ALL, THETA1_6, THETA2_6, THETA3_6); 290 | 291 | kinAngleArray_To_servoPwmArray(0, THETA1_6, DATA_POINT_ALL, PWM1_6); 292 | kinAngleArray_To_servoPwmArray(1, THETA2_6, DATA_POINT_ALL, PWM2_6); 293 | kinAngleArray_To_servoPwmArray(2, THETA3_6, DATA_POINT_ALL, PWM3_6); 294 | 295 | Serial.println("X6"); 296 | print_array(X6, DATA_POINT_ALL); 297 | Serial.println("Y6"); 298 | print_array(Y6, DATA_POINT_ALL); 299 | Serial.println("Z6"); 300 | print_array(Z6, DATA_POINT_ALL); 301 | 302 | Serial.println("THETA1_6"); 303 | print_array(THETA1_6, DATA_POINT_ALL); 304 | Serial.println("THETA2_6"); 305 | print_array(THETA2_6, DATA_POINT_ALL); 306 | Serial.println("THETA3_6"); 307 | print_array(THETA3_6, DATA_POINT_ALL); 308 | 309 | Serial.println("PWM1_6"); 310 | print_array(PWM1_6, DATA_POINT_ALL); 311 | Serial.println("PWM2_6"); 312 | print_array(PWM2_6, DATA_POINT_ALL); 313 | Serial.println("PWM3_6"); 314 | print_array(PWM3_6, DATA_POINT_ALL); 315 | 316 | 317 | 318 | pwm.writeMicroseconds(0, int(pwm1_mid)); 319 | pwm.writeMicroseconds(1, int(pwm2_mid)); 320 | pwm.writeMicroseconds(2, int(pwm3_mid)); 321 | 322 | pwm.writeMicroseconds(3, int(pwm1_mid)); 323 | pwm.writeMicroseconds(4, int(pwm2_mid)); 324 | pwm.writeMicroseconds(5, int(pwm3_mid)); 325 | 326 | pwm.writeMicroseconds(15, int(pwm1_mid)); 327 | pwm_2.writeMicroseconds(0, int(pwm2_mid)); 328 | pwm_2.writeMicroseconds(1, int(pwm3_mid)); 329 | 330 | delay(3000); 331 | 332 | 333 | 334 | 335 | } 336 | 337 | void loop() { 338 | 339 | if (counter < DATA_POINT_ALL) { 340 | 341 | pwm.writeMicroseconds(0, PWM1_1[counter]); 342 | pwm.writeMicroseconds(1, PWM2_1[counter]); 343 | pwm.writeMicroseconds(2, PWM3_1[counter]); 344 | 345 | pwm.writeMicroseconds(3, PWM1_2[counter]); 346 | pwm.writeMicroseconds(4, PWM2_2[counter]); 347 | pwm.writeMicroseconds(5, PWM3_2[counter]); 348 | 349 | pwm.writeMicroseconds(15, PWM1_6[counter]); 350 | pwm_2.writeMicroseconds(0, PWM2_6[counter]); 351 | pwm_2.writeMicroseconds(1, PWM3_6[counter]); 352 | 353 | 354 | counter++; 355 | 356 | } else { 357 | 358 | counter = 0; 359 | } 360 | 361 | delay(5); 362 | 363 | 364 | } 365 | 366 | 367 | void inv(double x, double y, double z, double& theta1, double& theta2, double& theta3) { 368 | theta1 = atan(y / x); 369 | double r2 = (x / cos(theta1)) - L1; 370 | double phi2 = atan(z / r2); 371 | double r1 = sqrt(pow(r2, 2) + pow(z, 2)); 372 | double phi1 = acos(-( (pow(L3, 2) - pow(L2, 2) - pow(r1, 2)) / (2 * L2 * r1) )); 373 | 374 | theta2 = phi1 + phi2; 375 | 376 | double phi3 = acos(-( (pow(r1, 2) - pow(L2, 2) - pow(L3, 2)) / (2 * L2 * L3) )); 377 | 378 | theta3 = -(PI - phi3); 379 | } 380 | 381 | void linspace(double Start, double Stop, int Size, double* Arr) { 382 | 383 | double increment = (Stop - Start) / (Size - 1); 384 | 385 | for (int i = 0; i < Size ; i++) { 386 | 387 | if (i == 0) { 388 | Arr[i] = Start; 389 | } else { 390 | Arr[i] = Arr[i - 1] + increment; 391 | } 392 | 393 | } 394 | } 395 | 396 | void print_array(double* arr, int len) { 397 | 398 | for (int i = 0; i < len; i++) { 399 | Serial.print(arr[i]); 400 | Serial.print(" "); 401 | } 402 | Serial.println(" "); 403 | 404 | } 405 | 406 | void print_array(int* arr, int len) { 407 | 408 | for (int i = 0; i < len; i++) { 409 | Serial.print(arr[i]); 410 | Serial.print(" "); 411 | } 412 | Serial.println(" "); 413 | 414 | } 415 | 416 | void concat_arrays(double* arr1, double* arr2, int arr1_len, int arr2_len, double* arr3) { 417 | 418 | for (int i = 0; i < arr1_len; i++) { 419 | arr3[i] = arr1[i]; 420 | } 421 | 422 | for (int i = 0; i < arr2_len; i++) { 423 | arr3[arr1_len + i] = arr2[i]; 424 | } 425 | } 426 | 427 | void inv_arrays(double* X, double* Y, double* Z, int len, double* THETA1, double* THETA2, double* THETA3) { 428 | 429 | for (int i = 0; i < len; i++) { 430 | double the1; 431 | double the2; 432 | double the3; 433 | 434 | inv(X[i], Y[i], Z[i], the1, the2, the3); 435 | 436 | THETA1[i] = the1 * RAD2DEG; 437 | THETA2[i] = the2 * RAD2DEG; 438 | THETA3[i] = the3 * RAD2DEG; 439 | } 440 | 441 | } 442 | 443 | double map_with_limit(double val, double in_min, double in_max, double out_min, double out_max) { 444 | 445 | double m; 446 | double out; 447 | 448 | m = (out_max - out_min) / (in_max - in_min); 449 | out = m * (val - in_min) + out_min; 450 | 451 | if (out_min > out_max) { 452 | if (out > out_min) { 453 | out = out_min; 454 | } 455 | else if (out < out_max) { 456 | out = out_max; 457 | } 458 | 459 | } 460 | else if (out_max > out_min) { 461 | if (out > out_max) { 462 | out = out_max; 463 | } 464 | else if (out < out_min) { 465 | out = out_min; 466 | } 467 | } 468 | 469 | return out; 470 | 471 | } 472 | 473 | 474 | void kinAngleArray_To_servoPwmArray(int servo_id, double* THETA, int len, int* PWM) { 475 | 476 | double pwm_val; 477 | 478 | for (int i = 0; i < len; i++) { 479 | // if (servo_id == 0 || servo_id == 1) { 480 | // pwm_val = map_with_limit(THETA[i], -70.0, 70.0, 800.0, 2200.0); 481 | // } else { 482 | // pwm_val = map_with_limit(THETA[i], -160.0, -20.0, 2200.0, 800.0); 483 | // } 484 | 485 | if (servo_id == 0) { 486 | pwm_val = map_with_limit(THETA[i], -90.0, 90.0, pwm1_min, pwm1_max); 487 | } else if (servo_id == 1) { 488 | pwm_val = map_with_limit(THETA[i], -70.0, 70.0, pwm2_min, pwm2_max); 489 | } else { 490 | pwm_val = map_with_limit(THETA[i], -160.0, -20.0, pwm3_min, pwm3_max); 491 | } 492 | 493 | PWM[i] = int(pwm_val); 494 | 495 | } 496 | 497 | } 498 | -------------------------------------------------------------------------------- /test_codes/test_normal_walking/test_normal_walking.ino: -------------------------------------------------------------------------------- 1 | #include "driver/uart.h" 2 | #include 3 | #include 4 | #include 5 | 6 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 7 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 8 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 9 | 10 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); 11 | Adafruit_PWMServoDriver pwm_2 = Adafruit_PWMServoDriver(0x41); 12 | 13 | HexapodLib h; 14 | int counter = 0; 15 | int ang_inc = 0; 16 | 17 | #define NUMERO_PORTA_SERIALE UART_NUM_2 18 | #define BUF_SIZE (1024 * 2) 19 | #define RD_BUF_SIZE (1024) 20 | static QueueHandle_t uart2_queue; 21 | 22 | static const char * TAG = ""; 23 | 24 | #define U2RXD 16 25 | #define U2TXD 17 26 | 27 | uint8_t rxbuf[256]; //Buffer di ricezione 28 | uint16_t rx_fifo_len; //Lunghezza dati 29 | 30 | 31 | bool led_state = true; 32 | uint16_t ch[16]; 33 | uint16_t checksum = 0; 34 | uint16_t sbus_ch[16]; 35 | 36 | int delay_time = 25; 37 | unsigned long period = 0; 38 | unsigned long last_stamp; 39 | bool from_walk = false; 40 | bool push_throttle = false; 41 | unsigned long push_period = 0; 42 | unsigned long last_push_thr_stamp = millis(); 43 | unsigned long throttle_trig = false; 44 | bool do_smooth = false; 45 | int str_index = 7; 46 | 47 | int sbus_max = 1680; 48 | int sbus_mid = 1024; 49 | int sbus_min = 368; 50 | int sbus_db_max = 1072; 51 | int sbus_db_min = 976; 52 | 53 | int max_delay = 50; 54 | int min_delay = 10; 55 | 56 | int pwm1_1; 57 | int pwm2_1; 58 | int pwm3_1; 59 | 60 | int pwm1_2; 61 | int pwm2_2; 62 | int pwm3_2; 63 | 64 | int pwm1_3; 65 | int pwm2_3; 66 | int pwm3_3; 67 | 68 | int pwm1_4; 69 | int pwm2_4; 70 | int pwm3_4; 71 | 72 | int pwm1_5; 73 | int pwm2_5; 74 | int pwm3_5; 75 | 76 | int pwm1_6; 77 | int pwm2_6; 78 | int pwm3_6; 79 | 80 | int pwm1_1_prev; 81 | int pwm2_1_prev; 82 | int pwm3_1_prev; 83 | 84 | int pwm1_2_prev; 85 | int pwm2_2_prev; 86 | int pwm3_2_prev; 87 | 88 | int pwm1_3_prev; 89 | int pwm2_3_prev; 90 | int pwm3_3_prev; 91 | 92 | int pwm1_4_prev; 93 | int pwm2_4_prev; 94 | int pwm3_4_prev; 95 | 96 | int pwm1_5_prev; 97 | int pwm2_5_prev; 98 | int pwm3_5_prev; 99 | 100 | int pwm1_6_prev; 101 | int pwm2_6_prev; 102 | int pwm3_6_prev; 103 | 104 | void setup() { 105 | Serial.begin(115200); 106 | 107 | uart_config_t Configurazione_UART2 = { 108 | .baud_rate = 115200, 109 | .data_bits = UART_DATA_8_BITS, 110 | .parity = UART_PARITY_DISABLE, 111 | .stop_bits = UART_STOP_BITS_1, 112 | .flow_ctrl = UART_HW_FLOWCTRL_DISABLE 113 | }; 114 | uart_param_config(NUMERO_PORTA_SERIALE, &Configurazione_UART2); 115 | 116 | esp_log_level_set(TAG, ESP_LOG_INFO); 117 | 118 | uart_set_pin(NUMERO_PORTA_SERIALE, U2TXD, U2RXD, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); 119 | 120 | uart_driver_install(NUMERO_PORTA_SERIALE, BUF_SIZE, BUF_SIZE, 20, &uart2_queue, 0); 121 | 122 | xTaskCreate(UART_ISR_ROUTINE, "UART_ISR_ROUTINE", 2048, NULL, 12, NULL); 123 | 124 | 125 | pwm.begin(); 126 | pwm.setOscillatorFrequency(27000000); 127 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 128 | delay(5); 129 | 130 | pwm_2.begin(); 131 | pwm_2.setOscillatorFrequency(27000000); 132 | pwm_2.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 133 | delay(5); 134 | h.generate_crabWalkingLUT(); 135 | h.generate_inplaceTurning_LUT(); 136 | h.generate_normalWalking_LUT(); 137 | 138 | setHomePosition(); 139 | init_global_PWM(); 140 | 141 | Serial.println("h.normalWalking_PWM_LUT[0][0]"); 142 | print_array(h.normalWalking_PWM_LUT[0][0], DATA_POINT_ALL); 143 | Serial.println("h.normalWalking_PWM_LUT[0][1]"); 144 | print_array(h.normalWalking_PWM_LUT[0][1], DATA_POINT_ALL); 145 | Serial.println("h.normalWalking_PWM_LUT[0][2]"); 146 | print_array(h.normalWalking_PWM_LUT[0][2], DATA_POINT_ALL); 147 | Serial.println(" "); 148 | Serial.println("h.normalWalking_PWM_LUT[1][0]"); 149 | print_array(h.normalWalking_PWM_LUT[1][0], DATA_POINT_ALL); 150 | Serial.println("h.normalWalking_PWM_LUT[1][4]"); 151 | print_array(h.normalWalking_PWM_LUT[1][4], DATA_POINT_ALL); 152 | Serial.println("h.normalWalking_PWM_LUT[1][10]"); 153 | print_array(h.normalWalking_PWM_LUT[1][10], DATA_POINT_ALL); 154 | Serial.println(" "); 155 | Serial.println("h.normalWalking_PWM_LUT[10][0]"); 156 | print_array(h.normalWalking_PWM_LUT[10][0], DATA_POINT_ALL); 157 | Serial.println("h.normalWalking_PWM_LUT[10][4]"); 158 | print_array(h.normalWalking_PWM_LUT[10][4], DATA_POINT_ALL); 159 | Serial.println("h.normalWalking_PWM_LUT[10][10]"); 160 | print_array(h.normalWalking_PWM_LUT[10][10], DATA_POINT_ALL); 161 | Serial.println(" "); 162 | 163 | last_stamp = millis(); 164 | delay(3000); 165 | } 166 | 167 | void loop() { 168 | 169 | memcpy(sbus_ch, ch, sizeof(ch)); 170 | 171 | delay_time = map(sbus_ch[1], sbus_db_max, sbus_max, max_delay, min_delay); 172 | if (sbus_ch[1] >= sbus_db_max) { 173 | 174 | if ((sbus_ch[0] < sbus_db_max) && (sbus_ch[0] > 1000)) { 175 | str_index = 7; 176 | } else if (sbus_ch[0] >= sbus_db_max) { 177 | str_index = (int)map(sbus_ch[0], sbus_mid, sbus_max, 7, 14); 178 | } else if (sbus_ch[0] <= sbus_db_min) { 179 | str_index = (int)map(sbus_ch[0], sbus_min, sbus_mid, 0, 7); 180 | } 181 | delay_time = map(sbus_ch[1], sbus_mid, sbus_max, max_delay, min_delay); 182 | 183 | } else if (sbus_ch[1] < sbus_db_min) { 184 | 185 | delay_time = map(sbus_ch[1], sbus_db_min, sbus_min, max_delay, min_delay); 186 | str_index = 7; 187 | 188 | } else { 189 | str_index = 7; 190 | } 191 | 192 | period = millis() - last_stamp; 193 | if ((period >= delay_time) && ((sbus_ch[1] < sbus_db_min) || (sbus_ch[1] > sbus_db_max)) ) { 194 | 195 | if (counter < DATA_POINT_ALL) { 196 | 197 | //turningInPlace(0, counter); 198 | //setInPlaceTurningPWM(0, counter); 199 | setNormalWalkingPWM(str_index, counter); 200 | counter++; 201 | 202 | } else { 203 | 204 | counter = 0; 205 | } 206 | 207 | last_stamp = millis(); 208 | from_walk = true; 209 | do_smooth = true; 210 | 211 | } else if ((sbus_ch[1] >= sbus_db_min) && (sbus_ch[1] <= sbus_db_max)) { 212 | 213 | //if (from_walk) { 214 | //setHomePosition(); 215 | //} 216 | 217 | setHomePWM(); 218 | counter = 0; 219 | from_walk = false; 220 | do_smooth = true; 221 | delay(25); 222 | 223 | } else { 224 | 225 | // Do nothing... 226 | } 227 | 228 | // Smoothing PWM values 229 | if (do_smooth == true) { 230 | Serial.println("Do smoothing"); 231 | pwm1_1 = smoothingPWM(pwm1_1, pwm1_1_prev); 232 | pwm2_1 = smoothingPWM(pwm2_1, pwm2_1_prev); 233 | pwm3_1 = smoothingPWM(pwm3_1, pwm3_1_prev); 234 | 235 | pwm1_2 = smoothingPWM(pwm1_2, pwm1_2_prev); 236 | pwm2_2 = smoothingPWM(pwm2_2, pwm2_2_prev); 237 | pwm3_2 = smoothingPWM(pwm3_2, pwm3_2_prev); 238 | 239 | pwm1_3 = smoothingPWM(pwm1_3, pwm1_3_prev); 240 | pwm2_3 = smoothingPWM(pwm2_3, pwm2_3_prev); 241 | pwm3_3 = smoothingPWM(pwm3_3, pwm3_3_prev); 242 | 243 | pwm1_4 = smoothingPWM(pwm1_4, pwm1_4_prev); 244 | pwm2_4 = smoothingPWM(pwm2_4, pwm2_4_prev); 245 | pwm3_4 = smoothingPWM(pwm3_4, pwm3_4_prev); 246 | 247 | pwm1_5 = smoothingPWM(pwm1_5, pwm1_5_prev); 248 | pwm2_5 = smoothingPWM(pwm2_5, pwm2_5_prev); 249 | pwm3_5 = smoothingPWM(pwm3_5, pwm3_5_prev); 250 | 251 | pwm1_6 = smoothingPWM(pwm1_6, pwm1_6_prev); 252 | pwm2_6 = smoothingPWM(pwm2_6, pwm2_6_prev); 253 | pwm3_6 = smoothingPWM(pwm3_6, pwm3_6_prev); 254 | } 255 | 256 | pwm1_1_prev = pwm1_1; 257 | pwm2_1_prev = pwm2_1; 258 | pwm3_1_prev = pwm3_1; 259 | 260 | pwm1_2_prev = pwm1_2; 261 | pwm2_2_prev = pwm2_2; 262 | pwm3_2_prev = pwm3_2; 263 | 264 | pwm1_3_prev = pwm1_3; 265 | pwm2_3_prev = pwm2_3; 266 | pwm3_3_prev = pwm3_3; 267 | 268 | pwm1_4_prev = pwm1_4; 269 | pwm2_4_prev = pwm2_4; 270 | pwm3_4_prev = pwm3_4; 271 | 272 | pwm1_5_prev = pwm1_5; 273 | pwm2_5_prev = pwm2_5; 274 | pwm3_5_prev = pwm3_5; 275 | 276 | pwm1_6_prev = pwm1_6; 277 | pwm2_6_prev = pwm2_6; 278 | pwm3_6_prev = pwm3_6; 279 | 280 | driveServo(); 281 | Serial.print("counter "); 282 | Serial.print(counter); 283 | Serial.print(" delay_time "); 284 | Serial.print(delay_time); 285 | Serial.print(" str_index "); 286 | Serial.println(str_index); 287 | delay(1); 288 | 289 | 290 | } 291 | 292 | void print_array(double* arr, int len) { 293 | 294 | for (int i = 0; i < len; i++) { 295 | Serial.print(arr[i]); 296 | Serial.print(" "); 297 | } 298 | Serial.println(" "); 299 | 300 | } 301 | 302 | 303 | void print_array(float* arr, int len) { 304 | 305 | for (int i = 0; i < len; i++) { 306 | Serial.print(arr[i]); 307 | Serial.print(" "); 308 | } 309 | Serial.println(" "); 310 | 311 | } 312 | 313 | void print_array(int* arr, int len) { 314 | 315 | for (int i = 0; i < len; i++) { 316 | Serial.print(arr[i]); 317 | Serial.print(" "); 318 | } 319 | Serial.println(" "); 320 | 321 | } 322 | static void UART_ISR_ROUTINE(void *pvParameters) 323 | { 324 | uart_event_t event; 325 | size_t buffered_size; 326 | bool exit_condition = false; 327 | 328 | //Infinite loop to run main bulk of task 329 | while (1) { 330 | 331 | //Loop will continually block (i.e. wait) on event messages from the event queue 332 | if (xQueueReceive(uart2_queue, (void * )&event, (portTickType)portMAX_DELAY)) { 333 | 334 | //Handle received event 335 | if (event.type == UART_DATA) { 336 | 337 | uint8_t UART2_data[128]; 338 | //uint8_t buf[35]; 339 | //uint16_t ch[16]; 340 | int UART2_data_length = 0; 341 | ESP_ERROR_CHECK(uart_get_buffered_data_len(UART_NUM_2, (size_t*)&UART2_data_length)); 342 | UART2_data_length = uart_read_bytes(UART_NUM_2, UART2_data, UART2_data_length, 100); 343 | 344 | //Serial.println("LEN= ");Serial.println(UART2_data_length); 345 | 346 | //Serial.print("DATA= "); 347 | //for(byte i=0; i 2 | #include 3 | 4 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 5 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 6 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 7 | 8 | #define L1 60 //40 9 | #define L2 80 10 | #define L3 171 //163 11 | 12 | #define RAD2DEG 180/PI 13 | 14 | #define S -150 // starting point offset from 0 15 | #define T 100 // step distance (how far the foot will move) 16 | #define A 50 // step height (how the foot will lift from ground) 17 | #define DATA_POINT_PER_LINE 50 18 | #define DATA_POINT_ALL DATA_POINT_PER_LINE*2 19 | 20 | #define pwm1_min 800.0 21 | #define pwm1_mid 1350.0 22 | #define pwm1_max 1900.0 23 | 24 | #define pwm2_min 800.0 25 | #define pwm2_mid 1500.0 26 | #define pwm2_max 2200.0 27 | 28 | #define pwm3_min 800.0 29 | #define pwm3_mid 1500.0 30 | #define pwm3_max 2200.0 31 | 32 | 33 | double x = 150.0; 34 | double y = 100.0; 35 | double z = -100.0; 36 | 37 | double x_start = 160.0; 38 | 39 | double theta1; 40 | double theta2; 41 | double theta3; 42 | 43 | double X_line[DATA_POINT_PER_LINE]; 44 | double Y_line[DATA_POINT_PER_LINE]; 45 | double Z_line[DATA_POINT_PER_LINE]; 46 | 47 | double X_curve[DATA_POINT_PER_LINE]; 48 | double Y_curve[DATA_POINT_PER_LINE]; 49 | double Z_curve[DATA_POINT_PER_LINE]; 50 | 51 | double X[DATA_POINT_ALL]; 52 | double Y[DATA_POINT_ALL]; 53 | double Z[DATA_POINT_ALL]; 54 | 55 | double P1[2] = { -T / 2, S}; 56 | double P2[2] = {0, S + (2 * A)}; 57 | double P3[2] = {T / 2, S}; 58 | double t[DATA_POINT_PER_LINE]; 59 | 60 | double THETA1[DATA_POINT_ALL]; 61 | double THETA2[DATA_POINT_ALL]; 62 | double THETA3[DATA_POINT_ALL]; 63 | 64 | int PWM1[DATA_POINT_ALL]; 65 | int PWM2[DATA_POINT_ALL]; 66 | int PWM3[DATA_POINT_ALL]; 67 | 68 | int counter = 0; 69 | 70 | // sizeof() returns number of bytes not array's length 71 | 72 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); 73 | 74 | void setup() { 75 | 76 | Serial.begin(115200); 77 | pwm.begin(); 78 | 79 | pwm.setOscillatorFrequency(27000000); 80 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 81 | 82 | delay(5); 83 | 84 | linspace(x_start, x_start, DATA_POINT_PER_LINE, X_line); 85 | linspace(S, S, DATA_POINT_PER_LINE, Z_line); 86 | 87 | //inv(x, y, z, theta1, theta2, theta3); 88 | 89 | linspace(T / 2, -T / 2, DATA_POINT_PER_LINE, Y_line); 90 | 91 | // Create Bezier curve 92 | linspace(0.0, 1.0, DATA_POINT_PER_LINE, t); 93 | linspace(x_start, x_start, DATA_POINT_PER_LINE, X_curve); 94 | for (int i = 0; i < DATA_POINT_PER_LINE; i++) { 95 | Y_curve[i] = (pow((1 - t[i]), 2) * P1[0]) + (2 * (1 - t[i]) * t[i] * P2[0]) + (pow(t[i], 2) * P3[0]); 96 | Z_curve[i] = (pow((1 - t[i]), 2) * P1[1]) + (2 * (1 - t[i]) * t[i] * P2[1]) + (pow(t[i], 2) * P3[1]); 97 | } 98 | 99 | 100 | concat_arrays(X_curve, X_line, DATA_POINT_PER_LINE, DATA_POINT_PER_LINE, X); 101 | concat_arrays(Y_curve, Y_line, DATA_POINT_PER_LINE, DATA_POINT_PER_LINE, Y); 102 | concat_arrays(Z_curve, Z_line, DATA_POINT_PER_LINE, DATA_POINT_PER_LINE, Z); 103 | 104 | inv_arrays(X, Y, Z, DATA_POINT_ALL, THETA1, THETA2, THETA3); 105 | 106 | kinAngleArray_To_servoPwmArray(0, THETA1, DATA_POINT_ALL, PWM1); 107 | kinAngleArray_To_servoPwmArray(1, THETA2, DATA_POINT_ALL, PWM2); 108 | kinAngleArray_To_servoPwmArray(2, THETA3, DATA_POINT_ALL, PWM3); 109 | 110 | Serial.println("t"); 111 | print_array(t, DATA_POINT_PER_LINE); 112 | Serial.println("X_line"); 113 | print_array(X_line, DATA_POINT_PER_LINE); 114 | Serial.println("Y_line"); 115 | print_array(Y_line, DATA_POINT_PER_LINE); 116 | Serial.println("Z_line"); 117 | print_array(Z_line, DATA_POINT_PER_LINE); 118 | Serial.println("X_curve"); 119 | print_array(X_curve, DATA_POINT_PER_LINE); 120 | Serial.println("Y_curve"); 121 | print_array(Y_curve, DATA_POINT_PER_LINE); 122 | Serial.println("Z_curve"); 123 | print_array(Z_curve, DATA_POINT_PER_LINE); 124 | Serial.println("X"); 125 | print_array(X, DATA_POINT_ALL); 126 | Serial.println("Y"); 127 | print_array(Y, DATA_POINT_ALL); 128 | Serial.println("Z"); 129 | print_array(Z, DATA_POINT_ALL); 130 | 131 | Serial.println("THETA1"); 132 | print_array(THETA1, DATA_POINT_ALL); 133 | Serial.println("THETA2"); 134 | print_array(THETA2, DATA_POINT_ALL); 135 | Serial.println("THETA3"); 136 | print_array(THETA3, DATA_POINT_ALL); 137 | 138 | Serial.println("PWM1"); 139 | print_array(PWM1, DATA_POINT_ALL); 140 | Serial.println("PWM2"); 141 | print_array(PWM2, DATA_POINT_ALL); 142 | Serial.println("PWM3"); 143 | print_array(PWM3, DATA_POINT_ALL); 144 | 145 | pwm.writeMicroseconds(0, int(pwm1_mid)); 146 | pwm.writeMicroseconds(1, int(pwm2_mid)); 147 | pwm.writeMicroseconds(2, int(pwm3_mid)); 148 | 149 | delay(3000); 150 | 151 | 152 | 153 | 154 | } 155 | 156 | void loop() { 157 | 158 | if (counter < DATA_POINT_ALL) { 159 | 160 | pwm.writeMicroseconds(0, PWM1[counter]); 161 | // pwm.writeMicroseconds(0, 1350);/ 162 | pwm.writeMicroseconds(1, PWM2[counter]); 163 | pwm.writeMicroseconds(2, PWM3[counter]); 164 | 165 | counter++; 166 | 167 | } else { 168 | 169 | counter = 0; 170 | } 171 | 172 | delay(5); 173 | 174 | 175 | } 176 | 177 | 178 | void inv(double x, double y, double z, double& theta1, double& theta2, double& theta3) { 179 | theta1 = atan(y / x); 180 | double r2 = (x / cos(theta1)) - L1; 181 | double phi2 = atan(z / r2); 182 | double r1 = sqrt(pow(r2, 2) + pow(z, 2)); 183 | double phi1 = acos(-( (pow(L3, 2) - pow(L2, 2) - pow(r1, 2)) / (2 * L2 * r1) )); 184 | 185 | theta2 = phi1 + phi2; 186 | 187 | double phi3 = acos(-( (pow(r1, 2) - pow(L2, 2) - pow(L3, 2)) / (2 * L2 * L3) )); 188 | 189 | theta3 = -(PI - phi3); 190 | } 191 | 192 | void linspace(double Start, double Stop, int Size, double* Arr) { 193 | 194 | double increment = (Stop - Start) / (Size - 1); 195 | 196 | for (int i = 0; i < Size ; i++) { 197 | 198 | if (i == 0) { 199 | Arr[i] = Start; 200 | } else { 201 | Arr[i] = Arr[i - 1] + increment; 202 | } 203 | 204 | } 205 | } 206 | 207 | void print_array(double* arr, int len) { 208 | 209 | for (int i = 0; i < len; i++) { 210 | Serial.print(arr[i]); 211 | Serial.print(" "); 212 | } 213 | Serial.println(" "); 214 | 215 | } 216 | 217 | void print_array(int* arr, int len) { 218 | 219 | for (int i = 0; i < len; i++) { 220 | Serial.print(arr[i]); 221 | Serial.print(" "); 222 | } 223 | Serial.println(" "); 224 | 225 | } 226 | 227 | void concat_arrays(double* arr1, double* arr2, int arr1_len, int arr2_len, double* arr3) { 228 | 229 | for (int i = 0; i < arr1_len; i++) { 230 | arr3[i] = arr1[i]; 231 | } 232 | 233 | for (int i = 0; i < arr2_len; i++) { 234 | arr3[arr1_len + i] = arr2[i]; 235 | } 236 | } 237 | 238 | void inv_arrays(double* X, double* Y, double* Z, int len, double* THETA1, double* THETA2, double* THETA3) { 239 | 240 | for (int i = 0; i < len; i++) { 241 | double the1; 242 | double the2; 243 | double the3; 244 | 245 | inv(X[i], Y[i], Z[i], the1, the2, the3); 246 | 247 | THETA1[i] = the1 * RAD2DEG; 248 | THETA2[i] = the2 * RAD2DEG; 249 | THETA3[i] = the3 * RAD2DEG; 250 | } 251 | 252 | } 253 | 254 | double map_with_limit(double val, double in_min, double in_max, double out_min, double out_max) { 255 | 256 | double m; 257 | double out; 258 | 259 | m = (out_max - out_min) / (in_max - in_min); 260 | out = m * (val - in_min) + out_min; 261 | 262 | if (out_min > out_max) { 263 | if (out > out_min) { 264 | out = out_min; 265 | } 266 | else if (out < out_max) { 267 | out = out_max; 268 | } 269 | 270 | } 271 | else if (out_max > out_min) { 272 | if (out > out_max) { 273 | out = out_max; 274 | } 275 | else if (out < out_min) { 276 | out = out_min; 277 | } 278 | } 279 | 280 | return out; 281 | 282 | } 283 | 284 | 285 | void kinAngleArray_To_servoPwmArray(int servo_id, double* THETA, int len, int* PWM) { 286 | 287 | double pwm_val; 288 | 289 | for (int i = 0; i < len; i++) { 290 | // if (servo_id == 0 || servo_id == 1) { 291 | // pwm_val = map_with_limit(THETA[i], -70.0, 70.0, 800.0, 2200.0); 292 | // } else { 293 | // pwm_val = map_with_limit(THETA[i], -160.0, -20.0, 2200.0, 800.0); 294 | // } 295 | 296 | if (servo_id == 0) { 297 | pwm_val = map_with_limit(THETA[i], -90.0, 90.0, pwm1_min, pwm1_max); 298 | } else if (servo_id == 1) { 299 | pwm_val = map_with_limit(THETA[i], -70.0, 70.0, pwm2_min, pwm2_max); 300 | } else { 301 | pwm_val = map_with_limit(THETA[i], -160.0, -20.0, pwm3_min, pwm3_max); 302 | } 303 | 304 | PWM[i] = int(pwm_val); 305 | 306 | } 307 | 308 | } 309 | -------------------------------------------------------------------------------- /test_codes/test_print_inplace_turning/test_print_inplace_turning.ino: -------------------------------------------------------------------------------- 1 | #include "driver/uart.h" 2 | #include 3 | #include 4 | #include 5 | 6 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 7 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 8 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 9 | 10 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); 11 | Adafruit_PWMServoDriver pwm_2 = Adafruit_PWMServoDriver(0x41); 12 | 13 | HexapodLib h; 14 | int counter = 0; 15 | int ang_inc = 0; 16 | 17 | #define NUMERO_PORTA_SERIALE UART_NUM_2 18 | #define BUF_SIZE (1024 * 2) 19 | #define RD_BUF_SIZE (1024) 20 | static QueueHandle_t uart2_queue; 21 | 22 | static const char * TAG = ""; 23 | 24 | #define U2RXD 16 25 | #define U2TXD 17 26 | 27 | uint8_t rxbuf[256]; //Buffer di ricezione 28 | uint16_t rx_fifo_len; //Lunghezza dati 29 | 30 | 31 | bool led_state = true; 32 | uint16_t ch[16]; 33 | uint16_t checksum = 0; 34 | uint16_t sbus_ch[16]; 35 | 36 | int delay_time = 5; 37 | unsigned long period = 0; 38 | unsigned long last_stamp; 39 | bool from_walk = false; 40 | bool push_throttle = false; 41 | unsigned long push_period = 0; 42 | unsigned long last_push_thr_stamp = millis(); 43 | unsigned long throttle_trig = false; 44 | bool do_smooth = false; 45 | 46 | int pwm1_1; 47 | int pwm2_1; 48 | int pwm3_1; 49 | 50 | int pwm1_2; 51 | int pwm2_2; 52 | int pwm3_2; 53 | 54 | int pwm1_3; 55 | int pwm2_3; 56 | int pwm3_3; 57 | 58 | int pwm1_4; 59 | int pwm2_4; 60 | int pwm3_4; 61 | 62 | int pwm1_5; 63 | int pwm2_5; 64 | int pwm3_5; 65 | 66 | int pwm1_6; 67 | int pwm2_6; 68 | int pwm3_6; 69 | 70 | int pwm1_1_prev; 71 | int pwm2_1_prev; 72 | int pwm3_1_prev; 73 | 74 | int pwm1_2_prev; 75 | int pwm2_2_prev; 76 | int pwm3_2_prev; 77 | 78 | int pwm1_3_prev; 79 | int pwm2_3_prev; 80 | int pwm3_3_prev; 81 | 82 | int pwm1_4_prev; 83 | int pwm2_4_prev; 84 | int pwm3_4_prev; 85 | 86 | int pwm1_5_prev; 87 | int pwm2_5_prev; 88 | int pwm3_5_prev; 89 | 90 | int pwm1_6_prev; 91 | int pwm2_6_prev; 92 | int pwm3_6_prev; 93 | 94 | void setup() { 95 | Serial.begin(115200); 96 | 97 | uart_config_t Configurazione_UART2 = { 98 | .baud_rate = 115200, 99 | .data_bits = UART_DATA_8_BITS, 100 | .parity = UART_PARITY_DISABLE, 101 | .stop_bits = UART_STOP_BITS_1, 102 | .flow_ctrl = UART_HW_FLOWCTRL_DISABLE 103 | }; 104 | uart_param_config(NUMERO_PORTA_SERIALE, &Configurazione_UART2); 105 | 106 | esp_log_level_set(TAG, ESP_LOG_INFO); 107 | 108 | uart_set_pin(NUMERO_PORTA_SERIALE, U2TXD, U2RXD, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); 109 | 110 | uart_driver_install(NUMERO_PORTA_SERIALE, BUF_SIZE, BUF_SIZE, 20, &uart2_queue, 0); 111 | 112 | xTaskCreate(UART_ISR_ROUTINE, "UART_ISR_ROUTINE", 2048, NULL, 12, NULL); 113 | 114 | 115 | pwm.begin(); 116 | pwm.setOscillatorFrequency(27000000); 117 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 118 | delay(5); 119 | 120 | pwm_2.begin(); 121 | pwm_2.setOscillatorFrequency(27000000); 122 | pwm_2.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 123 | delay(5); 124 | 125 | setHomePosition(); 126 | init_global_PWM(); 127 | last_stamp = millis(); 128 | delay(3000); 129 | } 130 | 131 | void loop() { 132 | // 133 | // memcpy(sbus_ch, ch, sizeof(ch)); 134 | // 135 | // period = millis() - last_stamp; 136 | // if ((period >= delay_time) && ((sbus_ch[0] < 900) || (sbus_ch[0] > 1070)) ) { 137 | // 138 | // if (counter < DATA_POINT_TURN_ALL) { 139 | // 140 | // if (sbus_ch[0] < 900) { 141 | // //turningInPlace(0, counter); 142 | // setInPlaceTurningPWM(0, counter); 143 | // counter++; 144 | // } else if (sbus_ch[0] > 1070) { 145 | // //turningInPlace(1, counter); 146 | // setInPlaceTurningPWM(1, counter); 147 | // counter++; 148 | // } 149 | // 150 | // 151 | // } else { 152 | // 153 | // counter = 0; 154 | // } 155 | // 156 | // last_stamp = millis(); 157 | // from_walk = true; 158 | // do_smooth = true; 159 | // 160 | // } else if ((sbus_ch[0] >= 900) && (sbus_ch[0] <= 1070)) { 161 | // 162 | // //if (from_walk) { 163 | // //setHomePosition(); 164 | // //} 165 | // 166 | // setHomePWM(); 167 | // counter = 0; 168 | // from_walk = false; 169 | // do_smooth = true; 170 | // delay(25); 171 | // 172 | // } else { 173 | // 174 | // // Do nothing... 175 | // } 176 | // 177 | // // Smoothing PWM values 178 | // if (do_smooth == true) { 179 | // Serial.println("Do smoothing"); 180 | // pwm1_1 = smoothingPWM(pwm1_1, pwm1_1_prev); 181 | // pwm2_1 = smoothingPWM(pwm2_1, pwm2_1_prev); 182 | // pwm3_1 = smoothingPWM(pwm3_1, pwm3_1_prev); 183 | // 184 | // pwm1_2 = smoothingPWM(pwm1_2, pwm1_2_prev); 185 | // pwm2_2 = smoothingPWM(pwm2_2, pwm2_2_prev); 186 | // pwm3_2 = smoothingPWM(pwm3_2, pwm3_2_prev); 187 | // 188 | // pwm1_3 = smoothingPWM(pwm1_3, pwm1_3_prev); 189 | // pwm2_3 = smoothingPWM(pwm2_3, pwm2_3_prev); 190 | // pwm3_3 = smoothingPWM(pwm3_3, pwm3_3_prev); 191 | // 192 | // pwm1_4 = smoothingPWM(pwm1_4, pwm1_4_prev); 193 | // pwm2_4 = smoothingPWM(pwm2_4, pwm2_4_prev); 194 | // pwm3_4 = smoothingPWM(pwm3_4, pwm3_4_prev); 195 | // 196 | // pwm1_5 = smoothingPWM(pwm1_5, pwm1_5_prev); 197 | // pwm2_5 = smoothingPWM(pwm2_5, pwm2_5_prev); 198 | // pwm3_5 = smoothingPWM(pwm3_5, pwm3_5_prev); 199 | // 200 | // pwm1_6 = smoothingPWM(pwm1_6, pwm1_6_prev); 201 | // pwm2_6 = smoothingPWM(pwm2_6, pwm2_6_prev); 202 | // pwm3_6 = smoothingPWM(pwm3_6, pwm3_6_prev); 203 | // } 204 | // 205 | // pwm1_1_prev = pwm1_1; 206 | // pwm2_1_prev = pwm2_1; 207 | // pwm3_1_prev = pwm3_1; 208 | // 209 | // pwm1_2_prev = pwm1_2; 210 | // pwm2_2_prev = pwm2_2; 211 | // pwm3_2_prev = pwm3_2; 212 | // 213 | // pwm1_3_prev = pwm1_3; 214 | // pwm2_3_prev = pwm2_3; 215 | // pwm3_3_prev = pwm3_3; 216 | // 217 | // pwm1_4_prev = pwm1_4; 218 | // pwm2_4_prev = pwm2_4; 219 | // pwm3_4_prev = pwm3_4; 220 | // 221 | // pwm1_5_prev = pwm1_5; 222 | // pwm2_5_prev = pwm2_5; 223 | // pwm3_5_prev = pwm3_5; 224 | // 225 | // pwm1_6_prev = pwm1_6; 226 | // pwm2_6_prev = pwm2_6; 227 | // pwm3_6_prev = pwm3_6; 228 | // 229 | // driveServo(); 230 | // Serial.print("counter "); 231 | // Serial.println(counter); 232 | // delay(1); 233 | 234 | 235 | } 236 | 237 | void print_array(double* arr, int len) { 238 | 239 | for (int i = 0; i < len; i++) { 240 | Serial.print(arr[i]); 241 | Serial.print(" "); 242 | } 243 | Serial.println(" "); 244 | 245 | } 246 | 247 | 248 | void print_array(float* arr, int len) { 249 | 250 | for (int i = 0; i < len; i++) { 251 | Serial.print(arr[i]); 252 | Serial.print(" "); 253 | } 254 | Serial.println(" "); 255 | 256 | } 257 | 258 | void print_array(int* arr, int len) { 259 | 260 | for (int i = 0; i < len; i++) { 261 | Serial.print(arr[i]); 262 | Serial.print(" "); 263 | } 264 | Serial.println(" "); 265 | 266 | } 267 | static void UART_ISR_ROUTINE(void *pvParameters) 268 | { 269 | uart_event_t event; 270 | size_t buffered_size; 271 | bool exit_condition = false; 272 | 273 | //Infinite loop to run main bulk of task 274 | while (1) { 275 | 276 | //Loop will continually block (i.e. wait) on event messages from the event queue 277 | if (xQueueReceive(uart2_queue, (void * )&event, (portTickType)portMAX_DELAY)) { 278 | 279 | //Handle received event 280 | if (event.type == UART_DATA) { 281 | 282 | uint8_t UART2_data[128]; 283 | //uint8_t buf[35]; 284 | //uint16_t ch[16]; 285 | int UART2_data_length = 0; 286 | ESP_ERROR_CHECK(uart_get_buffered_data_len(UART_NUM_2, (size_t*)&UART2_data_length)); 287 | UART2_data_length = uart_read_bytes(UART_NUM_2, UART2_data, UART2_data_length, 100); 288 | 289 | //Serial.println("LEN= ");Serial.println(UART2_data_length); 290 | 291 | //Serial.print("DATA= "); 292 | //for(byte i=0; i 3 | #include 4 | #include 5 | #include 6 | 7 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 8 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 9 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 10 | 11 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); 12 | Adafruit_PWMServoDriver pwm_2 = Adafruit_PWMServoDriver(0x41); 13 | 14 | HexapodLib h; 15 | int counter = 0; 16 | int ang_inc = 0; 17 | 18 | 19 | #define NUMERO_PORTA_SERIALE UART_NUM_2 20 | #define BUF_SIZE (1024 * 2) 21 | #define RD_BUF_SIZE (1024) 22 | static QueueHandle_t uart2_queue; 23 | 24 | static const char * TAG = ""; 25 | 26 | #define U2RXD 16 27 | #define U2TXD 17 28 | 29 | uint8_t rxbuf[256]; //Buffer di ricezione 30 | uint16_t rx_fifo_len; //Lunghezza dati 31 | 32 | 33 | bool led_state = true; 34 | uint16_t ch[16]; 35 | uint16_t checksum = 0; 36 | uint16_t sbus_ch[16]; 37 | 38 | int delay_time = 25; 39 | unsigned long period = 0; 40 | unsigned long last_stamp; 41 | bool from_walk = false; 42 | bool push_throttle = false; 43 | unsigned long push_period = 0; 44 | unsigned long last_push_thr_stamp = millis(); 45 | unsigned long throttle_trig = false; 46 | bool walk_mode; 47 | float roll = 0.0 * DEG2RAD; 48 | float pitch = 0.0 * DEG2RAD; 49 | float yaw = 0.0 * DEG2RAD; 50 | float x_trans = 0.0; 51 | float y_trans = 0.0; 52 | float z_trans = 0.0; 53 | float leg1_XYZ[3]; 54 | float leg2_XYZ[3]; 55 | float leg3_XYZ[3]; 56 | float leg4_XYZ[3]; 57 | float leg5_XYZ[3]; 58 | float leg6_XYZ[3]; 59 | 60 | float leg1_the1; 61 | float leg1_the2; 62 | float leg1_the3; 63 | 64 | float leg2_the1; 65 | float leg2_the2; 66 | float leg2_the3; 67 | 68 | float leg3_the1; 69 | float leg3_the2; 70 | float leg3_the3; 71 | 72 | float leg4_the1; 73 | float leg4_the2; 74 | float leg4_the3; 75 | 76 | float leg5_the1; 77 | float leg5_the2; 78 | float leg5_the3; 79 | 80 | float leg6_the1; 81 | float leg6_the2; 82 | float leg6_the3; 83 | 84 | int leg1_PWM1; 85 | int leg1_PWM2; 86 | int leg1_PWM3; 87 | 88 | int leg2_PWM1; 89 | int leg2_PWM2; 90 | int leg2_PWM3; 91 | 92 | int leg3_PWM1; 93 | int leg3_PWM2; 94 | int leg3_PWM3; 95 | 96 | int leg4_PWM1; 97 | int leg4_PWM2; 98 | int leg4_PWM3; 99 | 100 | int leg5_PWM1; 101 | int leg5_PWM2; 102 | int leg5_PWM3; 103 | 104 | int leg6_PWM1; 105 | int leg6_PWM2; 106 | int leg6_PWM3; 107 | 108 | //// JSON //// 109 | //const int capacity = JSON_ARRAY_SIZE(2) + 2*JSON_OBJECT_SIZE(2); 110 | //StaticJsonDocument doc; 111 | //calculate capacity size by here https://arduinojson.org/v6/assistant/ 112 | StaticJsonDocument<256> doc; 113 | JsonArray sbus = doc.createNestedArray("sbus"); 114 | JsonObject cmd_vel = doc.createNestedObject("cmd_vel"); 115 | 116 | void setup() { 117 | Serial.begin(115200); 118 | uart_config_t Configurazione_UART2 = { 119 | .baud_rate = 115200, 120 | .data_bits = UART_DATA_8_BITS, 121 | .parity = UART_PARITY_DISABLE, 122 | .stop_bits = UART_STOP_BITS_1, 123 | .flow_ctrl = UART_HW_FLOWCTRL_DISABLE 124 | }; 125 | uart_param_config(NUMERO_PORTA_SERIALE, &Configurazione_UART2); 126 | 127 | esp_log_level_set(TAG, ESP_LOG_INFO); 128 | 129 | uart_set_pin(NUMERO_PORTA_SERIALE, U2TXD, U2RXD, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); 130 | 131 | uart_driver_install(NUMERO_PORTA_SERIALE, BUF_SIZE, BUF_SIZE, 20, &uart2_queue, 0); 132 | 133 | xTaskCreate(UART_ISR_ROUTINE, "UART_ISR_ROUTINE", 2048, NULL, 12, NULL); 134 | 135 | Serial.begin(115200); 136 | 137 | pwm.begin(); 138 | pwm.setOscillatorFrequency(27000000); 139 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 140 | delay(5); 141 | 142 | pwm_2.begin(); 143 | pwm_2.setOscillatorFrequency(27000000); 144 | pwm_2.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 145 | delay(5); 146 | 147 | pwm.writeMicroseconds(0, h.PWM1_home); 148 | pwm.writeMicroseconds(1, h.PWM2_home); 149 | pwm.writeMicroseconds(2, h.PWM3_home); 150 | 151 | pwm.writeMicroseconds(3, h.PWM1_home); 152 | pwm.writeMicroseconds(4, h.PWM2_home); 153 | pwm.writeMicroseconds(5, h.PWM3_home); 154 | 155 | pwm.writeMicroseconds(6, h.PWM1_home); 156 | pwm.writeMicroseconds(7, h.PWM2_home); 157 | pwm.writeMicroseconds(8, h.PWM3_home); 158 | 159 | pwm.writeMicroseconds(9, h.PWM1_home); 160 | pwm.writeMicroseconds(10, h.PWM2_home); 161 | pwm.writeMicroseconds(11, h.PWM3_home); 162 | 163 | pwm.writeMicroseconds(12, h.PWM1_home); 164 | pwm.writeMicroseconds(13, h.PWM2_home); 165 | pwm.writeMicroseconds(14, h.PWM3_home); 166 | 167 | pwm.writeMicroseconds(15, h.PWM1_home); 168 | pwm_2.writeMicroseconds(0, h.PWM2_home); 169 | pwm_2.writeMicroseconds(1, h.PWM3_home); 170 | 171 | // roll = 0.0 * DEG2RAD; 172 | // pitch = 0.0 * DEG2RAD; 173 | // yaw = 0.0 * DEG2RAD; 174 | // h.bodyRotate_to_newLegXYZ(roll, pitch, yaw, leg1_XYZ, leg2_XYZ, leg3_XYZ, leg4_XYZ, leg5_XYZ, leg6_XYZ); 175 | x_trans = 0.0; 176 | y_trans = 0.0; 177 | z_trans = 0.0; 178 | h.bodyTranslate_to_newLegXYZ(x_trans, y_trans, z_trans, leg1_XYZ, leg2_XYZ, leg3_XYZ, leg4_XYZ, leg5_XYZ, leg6_XYZ); 179 | Serial.println(" "); 180 | Serial.print("leg1_XYZ "); 181 | print_array(leg1_XYZ, 3); 182 | Serial.print("leg2_XYZ "); 183 | print_array(leg2_XYZ, 3); 184 | Serial.print("leg3_XYZ "); 185 | print_array(leg3_XYZ, 3); 186 | Serial.print("leg4_XYZ "); 187 | print_array(leg4_XYZ, 3); 188 | Serial.print("leg5_XYZ "); 189 | print_array(leg5_XYZ, 3); 190 | Serial.print("leg6_XYZ "); 191 | print_array(leg6_XYZ, 3); 192 | Serial.println(" "); 193 | 194 | h.inv(leg1_XYZ[0], leg1_XYZ[1], leg1_XYZ[2], leg1_the1, leg1_the2, leg1_the3); 195 | h.inv(leg2_XYZ[0], leg2_XYZ[1], leg2_XYZ[2], leg2_the1, leg2_the2, leg2_the3); 196 | h.inv(leg3_XYZ[0], leg3_XYZ[1], leg3_XYZ[2], leg3_the1, leg3_the2, leg3_the3); 197 | h.inv(leg4_XYZ[0], leg4_XYZ[1], leg4_XYZ[2], leg4_the1, leg4_the2, leg4_the3); 198 | h.inv(leg5_XYZ[0], leg5_XYZ[1], leg5_XYZ[2], leg5_the1, leg5_the2, leg5_the3); 199 | h.inv(leg6_XYZ[0], leg6_XYZ[1], leg6_XYZ[2], leg6_the1, leg6_the2, leg6_the3); 200 | 201 | Serial.print("leg1 theta "); 202 | Serial.print(leg1_the1, 4); 203 | Serial.print(" "); 204 | Serial.print(leg1_the2, 4); 205 | Serial.print(" "); 206 | Serial.println(leg1_the3, 4); 207 | Serial.print("leg2 theta "); 208 | Serial.print(leg2_the1, 4); 209 | Serial.print(" "); 210 | Serial.print(leg2_the2, 4); 211 | Serial.print(" "); 212 | Serial.println(leg2_the3, 4); 213 | Serial.print("leg3 theta "); 214 | Serial.print(leg3_the1, 4); 215 | Serial.print(" "); 216 | Serial.print(leg3_the2, 4); 217 | Serial.print(" "); 218 | Serial.println(leg3_the3, 4); 219 | Serial.print("leg4 theta "); 220 | Serial.print(leg4_the1, 4); 221 | Serial.print(" "); 222 | Serial.print(leg4_the2, 4); 223 | Serial.print(" "); 224 | Serial.println(leg4_the3, 4); 225 | Serial.print("leg5 theta "); 226 | Serial.print(leg5_the1, 4); 227 | Serial.print(" "); 228 | Serial.print(leg5_the2, 4); 229 | Serial.print(" "); 230 | Serial.println(leg5_the3, 4); 231 | Serial.print("leg6 theta "); 232 | Serial.print(leg6_the1, 4); 233 | Serial.print(" "); 234 | Serial.print(leg6_the2, 4); 235 | Serial.print(" "); 236 | Serial.println(leg6_the3, 4); 237 | 238 | leg1_PWM1 = h.kinAngle_To_servoPwm(0, leg1_the1 * RAD2DEG); 239 | leg1_PWM2 = h.kinAngle_To_servoPwm(1, leg1_the2 * RAD2DEG); 240 | leg1_PWM3 = h.kinAngle_To_servoPwm(2, leg1_the3 * RAD2DEG); 241 | 242 | leg2_PWM1 = h.kinAngle_To_servoPwm(0, leg2_the1 * RAD2DEG); 243 | leg2_PWM2 = h.kinAngle_To_servoPwm(1, leg2_the2 * RAD2DEG); 244 | leg2_PWM3 = h.kinAngle_To_servoPwm(2, leg2_the3 * RAD2DEG); 245 | 246 | leg3_PWM1 = h.kinAngle_To_servoPwm(0, leg3_the1 * RAD2DEG); 247 | leg3_PWM2 = h.kinAngle_To_servoPwm(1, leg3_the2 * RAD2DEG); 248 | leg3_PWM3 = h.kinAngle_To_servoPwm(2, leg3_the3 * RAD2DEG); 249 | 250 | leg4_PWM1 = h.kinAngle_To_servoPwm(0, leg4_the1 * RAD2DEG); 251 | leg4_PWM2 = h.kinAngle_To_servoPwm(1, leg4_the2 * RAD2DEG); 252 | leg4_PWM3 = h.kinAngle_To_servoPwm(2, leg4_the3 * RAD2DEG); 253 | 254 | leg5_PWM1 = h.kinAngle_To_servoPwm(0, leg5_the1 * RAD2DEG); 255 | leg5_PWM2 = h.kinAngle_To_servoPwm(1, leg5_the2 * RAD2DEG); 256 | leg5_PWM3 = h.kinAngle_To_servoPwm(2, leg5_the3 * RAD2DEG); 257 | 258 | leg6_PWM1 = h.kinAngle_To_servoPwm(0, leg6_the1 * RAD2DEG); 259 | leg6_PWM2 = h.kinAngle_To_servoPwm(1, leg6_the2 * RAD2DEG); 260 | leg6_PWM3 = h.kinAngle_To_servoPwm(2, leg6_the3 * RAD2DEG); 261 | 262 | Serial.print("leg1 PWM "); 263 | Serial.print(leg1_PWM1); 264 | Serial.print(" "); 265 | Serial.print(leg1_PWM2); 266 | Serial.print(" "); 267 | Serial.println(leg1_PWM3); 268 | Serial.print("leg2 PWM "); 269 | Serial.print(leg2_PWM1); 270 | Serial.print(" "); 271 | Serial.print(leg2_PWM2); 272 | Serial.print(" "); 273 | Serial.println(leg2_PWM3); 274 | Serial.print("leg3 PWM "); 275 | Serial.print(leg3_PWM1); 276 | Serial.print(" "); 277 | Serial.print(leg3_PWM2); 278 | Serial.print(" "); 279 | Serial.println(leg3_PWM3); 280 | Serial.print("leg4 PWM "); 281 | Serial.print(leg4_PWM1); 282 | Serial.print(" "); 283 | Serial.print(leg4_PWM2); 284 | Serial.print(" "); 285 | Serial.println(leg4_PWM3); 286 | Serial.print("leg5 PWM "); 287 | Serial.print(leg5_PWM1); 288 | Serial.print(" "); 289 | Serial.print(leg5_PWM2); 290 | Serial.print(" "); 291 | Serial.println(leg5_PWM3); 292 | Serial.print("leg6 PWM "); 293 | Serial.print(leg6_PWM1); 294 | Serial.print(" "); 295 | Serial.print(leg6_PWM2); 296 | Serial.print(" "); 297 | Serial.println(leg6_PWM3); 298 | 299 | 300 | delay(3000); 301 | 302 | last_stamp = millis(); 303 | 304 | } 305 | 306 | void loop() { 307 | 308 | memcpy(sbus_ch, ch, sizeof(ch)); 309 | 310 | if (sbus_ch[6] > 1500) { 311 | delay_time = map(sbus_ch[1], 1070, 1680, 30, 5); 312 | if (sbus_ch[1] >= 1070) { 313 | 314 | if ((sbus_ch[0] < 1048) && (sbus_ch[0] > 1000)) { 315 | ang_inc = 0; 316 | } else if (sbus_ch[0] >= 1048) { 317 | ang_inc = (int)map(sbus_ch[0], 1048, 1680, 11, 6); 318 | } else if (sbus_ch[0] <= 1000) { 319 | ang_inc = (int)map(sbus_ch[0], 368, 980, 6, 0); 320 | } 321 | delay_time = map(sbus_ch[1], 1070, 1680, 30, 5); 322 | 323 | } else if (sbus_ch[1] < 980) { 324 | if ((sbus_ch[0] < 1048) && (sbus_ch[0] > 1000)) { 325 | ang_inc = 6; 326 | } else if (sbus_ch[0] >= 1048) { 327 | ang_inc = (int)map(sbus_ch[0], 1048, 1680, 6, 11); 328 | } else if (sbus_ch[0] <= 1000) { 329 | ang_inc = (int)map(sbus_ch[0], 368, 980, 0, 6); 330 | } 331 | 332 | delay_time = map(sbus_ch[1], 980, 368, 30, 5); 333 | 334 | } else { 335 | ang_inc = 0; 336 | } 337 | 338 | } else if (sbus_ch[6] > 900) { 339 | 340 | x_trans = (float)map(sbus_ch[3], 368, 1680, -80.0, 80.0); 341 | y_trans = (float)map(sbus_ch[1], 368, 1680, -80.0, 80.0); 342 | z_trans = (float)map(sbus_ch[0], 368, 1680, -50.0, 50.0); 343 | 344 | } else { 345 | 346 | roll = (float)map(sbus_ch[3], 368, 1680, -15.0, 15.0); 347 | pitch = (float)map(sbus_ch[1], 368, 1680, -15.0, 15.0); 348 | yaw = (float)map(sbus_ch[0], 368, 1680, -15.0, 15.0); 349 | 350 | } 351 | 352 | if (sbus_ch[6] > 1500) { 353 | Serial.print("sbus ch1 "); 354 | Serial.print(sbus_ch[0]); 355 | Serial.print(" sbus ch2 "); 356 | Serial.print(sbus_ch[1]); 357 | Serial.print(" ang_inc "); 358 | Serial.print(ang_inc); 359 | Serial.print(" counter "); 360 | Serial.print(counter); 361 | Serial.print(" delay time "); 362 | Serial.println(delay_time); 363 | 364 | } else if (sbus_ch[6] > 900) { 365 | 366 | Serial.print("sbus ch1 "); 367 | Serial.print(sbus_ch[0]); 368 | Serial.print(" sbus ch2 "); 369 | Serial.print(sbus_ch[1]); 370 | Serial.print(" sbus ch4 "); 371 | Serial.print(sbus_ch[3]); 372 | Serial.print(" x_trans "); 373 | Serial.print(x_trans); 374 | Serial.print(" y_trans "); 375 | Serial.print(y_trans); 376 | Serial.print(" z_trans "); 377 | Serial.print(z_trans); 378 | 379 | } else { 380 | 381 | Serial.print("sbus ch1 "); 382 | Serial.print(sbus_ch[0]); 383 | Serial.print(" sbus ch2 "); 384 | Serial.print(sbus_ch[1]); 385 | Serial.print(" sbus ch4 "); 386 | Serial.print(sbus_ch[3]); 387 | Serial.print(" roll "); 388 | Serial.print(roll); 389 | Serial.print(" pitch "); 390 | Serial.print(pitch); 391 | Serial.print(" yaw "); 392 | Serial.print(yaw); 393 | } 394 | 395 | 396 | if (sbus_ch[6] > 1500) { 397 | period = millis() - last_stamp; 398 | if ((period >= delay_time) && ((sbus_ch[1] >= 1070) || (sbus_ch[1] <= 980)) ) { 399 | 400 | if (counter < DATA_POINT_ALL) { 401 | 402 | crabWalking(ang_inc, counter); 403 | counter++; 404 | 405 | } else { 406 | 407 | counter = 0; 408 | } 409 | 410 | last_stamp = millis(); 411 | from_walk = true; 412 | 413 | 414 | } else if ((sbus_ch[1] < 1070) && (sbus_ch[1] > 980)) { 415 | 416 | if (from_walk) { 417 | setHomePosition(); 418 | } 419 | 420 | counter = 0; 421 | from_walk = false; 422 | delay(25); 423 | 424 | } else { 425 | // Do nothing... 426 | } 427 | } else if (sbus_ch[6] > 900) { 428 | h.bodyTranslate_to_newLegXYZ(x_trans, y_trans, z_trans, leg1_XYZ, leg2_XYZ, leg3_XYZ, leg4_XYZ, leg5_XYZ, leg6_XYZ); 429 | h.inv(leg1_XYZ[0], leg1_XYZ[1], leg1_XYZ[2], leg1_the1, leg1_the2, leg1_the3); 430 | h.inv(leg2_XYZ[0], leg2_XYZ[1], leg2_XYZ[2], leg2_the1, leg2_the2, leg2_the3); 431 | h.inv(leg3_XYZ[0], leg3_XYZ[1], leg3_XYZ[2], leg3_the1, leg3_the2, leg3_the3); 432 | h.inv(leg4_XYZ[0], leg4_XYZ[1], leg4_XYZ[2], leg4_the1, leg4_the2, leg4_the3); 433 | h.inv(leg5_XYZ[0], leg5_XYZ[1], leg5_XYZ[2], leg5_the1, leg5_the2, leg5_the3); 434 | h.inv(leg6_XYZ[0], leg6_XYZ[1], leg6_XYZ[2], leg6_the1, leg6_the2, leg6_the3); 435 | 436 | leg1_PWM1 = h.kinAngle_To_servoPwm(0, leg1_the1 * RAD2DEG); 437 | leg1_PWM2 = h.kinAngle_To_servoPwm(1, leg1_the2 * RAD2DEG); 438 | leg1_PWM3 = h.kinAngle_To_servoPwm(2, leg1_the3 * RAD2DEG); 439 | 440 | leg2_PWM1 = h.kinAngle_To_servoPwm(0, leg2_the1 * RAD2DEG); 441 | leg2_PWM2 = h.kinAngle_To_servoPwm(1, leg2_the2 * RAD2DEG); 442 | leg2_PWM3 = h.kinAngle_To_servoPwm(2, leg2_the3 * RAD2DEG); 443 | 444 | leg3_PWM1 = h.kinAngle_To_servoPwm(0, leg3_the1 * RAD2DEG); 445 | leg3_PWM2 = h.kinAngle_To_servoPwm(1, leg3_the2 * RAD2DEG); 446 | leg3_PWM3 = h.kinAngle_To_servoPwm(2, leg3_the3 * RAD2DEG); 447 | 448 | leg4_PWM1 = h.kinAngle_To_servoPwm(0, leg4_the1 * RAD2DEG); 449 | leg4_PWM2 = h.kinAngle_To_servoPwm(1, leg4_the2 * RAD2DEG); 450 | leg4_PWM3 = h.kinAngle_To_servoPwm(2, leg4_the3 * RAD2DEG); 451 | 452 | leg5_PWM1 = h.kinAngle_To_servoPwm(0, leg5_the1 * RAD2DEG); 453 | leg5_PWM2 = h.kinAngle_To_servoPwm(1, leg5_the2 * RAD2DEG); 454 | leg5_PWM3 = h.kinAngle_To_servoPwm(2, leg5_the3 * RAD2DEG); 455 | 456 | leg6_PWM1 = h.kinAngle_To_servoPwm(0, leg6_the1 * RAD2DEG); 457 | leg6_PWM2 = h.kinAngle_To_servoPwm(1, leg6_the2 * RAD2DEG); 458 | leg6_PWM3 = h.kinAngle_To_servoPwm(2, leg6_the3 * RAD2DEG); 459 | 460 | setBody(); 461 | } else { 462 | h.bodyRotate_to_newLegXYZ(roll * DEG2RAD, pitch * DEG2RAD, yaw * DEG2RAD, leg1_XYZ, leg2_XYZ, leg3_XYZ, leg4_XYZ, leg5_XYZ, leg6_XYZ); 463 | h.inv(leg1_XYZ[0], leg1_XYZ[1], leg1_XYZ[2], leg1_the1, leg1_the2, leg1_the3); 464 | h.inv(leg2_XYZ[0], leg2_XYZ[1], leg2_XYZ[2], leg2_the1, leg2_the2, leg2_the3); 465 | h.inv(leg3_XYZ[0], leg3_XYZ[1], leg3_XYZ[2], leg3_the1, leg3_the2, leg3_the3); 466 | h.inv(leg4_XYZ[0], leg4_XYZ[1], leg4_XYZ[2], leg4_the1, leg4_the2, leg4_the3); 467 | h.inv(leg5_XYZ[0], leg5_XYZ[1], leg5_XYZ[2], leg5_the1, leg5_the2, leg5_the3); 468 | h.inv(leg6_XYZ[0], leg6_XYZ[1], leg6_XYZ[2], leg6_the1, leg6_the2, leg6_the3); 469 | 470 | leg1_PWM1 = h.kinAngle_To_servoPwm(0, leg1_the1 * RAD2DEG); 471 | leg1_PWM2 = h.kinAngle_To_servoPwm(1, leg1_the2 * RAD2DEG); 472 | leg1_PWM3 = h.kinAngle_To_servoPwm(2, leg1_the3 * RAD2DEG); 473 | 474 | leg2_PWM1 = h.kinAngle_To_servoPwm(0, leg2_the1 * RAD2DEG); 475 | leg2_PWM2 = h.kinAngle_To_servoPwm(1, leg2_the2 * RAD2DEG); 476 | leg2_PWM3 = h.kinAngle_To_servoPwm(2, leg2_the3 * RAD2DEG); 477 | 478 | leg3_PWM1 = h.kinAngle_To_servoPwm(0, leg3_the1 * RAD2DEG); 479 | leg3_PWM2 = h.kinAngle_To_servoPwm(1, leg3_the2 * RAD2DEG); 480 | leg3_PWM3 = h.kinAngle_To_servoPwm(2, leg3_the3 * RAD2DEG); 481 | 482 | leg4_PWM1 = h.kinAngle_To_servoPwm(0, leg4_the1 * RAD2DEG); 483 | leg4_PWM2 = h.kinAngle_To_servoPwm(1, leg4_the2 * RAD2DEG); 484 | leg4_PWM3 = h.kinAngle_To_servoPwm(2, leg4_the3 * RAD2DEG); 485 | 486 | leg5_PWM1 = h.kinAngle_To_servoPwm(0, leg5_the1 * RAD2DEG); 487 | leg5_PWM2 = h.kinAngle_To_servoPwm(1, leg5_the2 * RAD2DEG); 488 | leg5_PWM3 = h.kinAngle_To_servoPwm(2, leg5_the3 * RAD2DEG); 489 | 490 | leg6_PWM1 = h.kinAngle_To_servoPwm(0, leg6_the1 * RAD2DEG); 491 | leg6_PWM2 = h.kinAngle_To_servoPwm(1, leg6_the2 * RAD2DEG); 492 | leg6_PWM3 = h.kinAngle_To_servoPwm(2, leg6_the3 * RAD2DEG); 493 | 494 | setBody(); 495 | } 496 | 497 | Serial.println(); 498 | delay(1); 499 | } 500 | 501 | static void UART_ISR_ROUTINE(void *pvParameters) 502 | { 503 | uart_event_t event; 504 | size_t buffered_size; 505 | bool exit_condition = false; 506 | 507 | //Infinite loop to run main bulk of task 508 | while (1) { 509 | 510 | //Loop will continually block (i.e. wait) on event messages from the event queue 511 | if (xQueueReceive(uart2_queue, (void * )&event, (portTickType)portMAX_DELAY)) { 512 | 513 | //Handle received event 514 | if (event.type == UART_DATA) { 515 | 516 | uint8_t UART2_data[128]; 517 | //uint8_t buf[35]; 518 | //uint16_t ch[16]; 519 | int UART2_data_length = 0; 520 | ESP_ERROR_CHECK(uart_get_buffered_data_len(UART_NUM_2, (size_t*)&UART2_data_length)); 521 | UART2_data_length = uart_read_bytes(UART_NUM_2, UART2_data, UART2_data_length, 100); 522 | 523 | //Serial.println("LEN= ");Serial.println(UART2_data_length); 524 | 525 | //Serial.print("DATA= "); 526 | //for(byte i=0; i 3 | #include 4 | #include 5 | #include 6 | 7 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 8 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 9 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 10 | 11 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); 12 | Adafruit_PWMServoDriver pwm_2 = Adafruit_PWMServoDriver(0x41); 13 | 14 | HexapodLib h; 15 | int counter = 0; 16 | int ang_inc = 0; 17 | 18 | 19 | #define NUMERO_PORTA_SERIALE UART_NUM_2 20 | #define BUF_SIZE (1024 * 2) 21 | #define RD_BUF_SIZE (1024) 22 | static QueueHandle_t uart2_queue; 23 | 24 | static const char * TAG = ""; 25 | 26 | #define U2RXD 16 27 | #define U2TXD 17 28 | 29 | uint8_t rxbuf[256]; //Buffer di ricezione 30 | uint16_t rx_fifo_len; //Lunghezza dati 31 | 32 | 33 | bool led_state = true; 34 | uint16_t ch[16]; 35 | uint16_t checksum = 0; 36 | uint16_t sbus_ch[16]; 37 | 38 | int delay_time = 25; 39 | unsigned long period = 0; 40 | unsigned long last_stamp; 41 | bool from_walk = false; 42 | bool push_throttle = false; 43 | unsigned long push_period = 0; 44 | unsigned long last_push_thr_stamp = millis(); 45 | unsigned long throttle_trig = false; 46 | 47 | //// JSON //// 48 | //const int capacity = JSON_ARRAY_SIZE(2) + 2*JSON_OBJECT_SIZE(2); 49 | //StaticJsonDocument doc; 50 | //calculate capacity size by here https://arduinojson.org/v6/assistant/ 51 | StaticJsonDocument<256> doc; 52 | JsonArray sbus = doc.createNestedArray("sbus"); 53 | JsonObject cmd_vel = doc.createNestedObject("cmd_vel"); 54 | 55 | void setup() { 56 | Serial.begin(115200); 57 | uart_config_t Configurazione_UART2 = { 58 | .baud_rate = 115200, 59 | .data_bits = UART_DATA_8_BITS, 60 | .parity = UART_PARITY_DISABLE, 61 | .stop_bits = UART_STOP_BITS_1, 62 | .flow_ctrl = UART_HW_FLOWCTRL_DISABLE 63 | }; 64 | uart_param_config(NUMERO_PORTA_SERIALE, &Configurazione_UART2); 65 | 66 | esp_log_level_set(TAG, ESP_LOG_INFO); 67 | 68 | uart_set_pin(NUMERO_PORTA_SERIALE, U2TXD, U2RXD, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); 69 | 70 | uart_driver_install(NUMERO_PORTA_SERIALE, BUF_SIZE, BUF_SIZE, 20, &uart2_queue, 0); 71 | 72 | xTaskCreate(UART_ISR_ROUTINE, "UART_ISR_ROUTINE", 2048, NULL, 12, NULL); 73 | 74 | Serial.begin(115200); 75 | 76 | pwm.begin(); 77 | pwm.setOscillatorFrequency(27000000); 78 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 79 | delay(5); 80 | 81 | pwm_2.begin(); 82 | pwm_2.setOscillatorFrequency(27000000); 83 | pwm_2.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 84 | delay(5); 85 | 86 | pwm.writeMicroseconds(0, h.PWM1_home); 87 | pwm.writeMicroseconds(1, h.PWM2_home); 88 | pwm.writeMicroseconds(2, h.PWM3_home); 89 | 90 | pwm.writeMicroseconds(3, h.PWM1_home); 91 | pwm.writeMicroseconds(4, h.PWM2_home); 92 | pwm.writeMicroseconds(5, h.PWM3_home); 93 | 94 | pwm.writeMicroseconds(6, h.PWM1_home); 95 | pwm.writeMicroseconds(7, h.PWM2_home); 96 | pwm.writeMicroseconds(8, h.PWM3_home); 97 | 98 | pwm.writeMicroseconds(9, h.PWM1_home); 99 | pwm.writeMicroseconds(10, h.PWM2_home); 100 | pwm.writeMicroseconds(11, h.PWM3_home); 101 | 102 | pwm.writeMicroseconds(12, h.PWM1_home); 103 | pwm.writeMicroseconds(13, h.PWM2_home); 104 | pwm.writeMicroseconds(14, h.PWM3_home); 105 | 106 | pwm.writeMicroseconds(15, h.PWM1_home); 107 | pwm_2.writeMicroseconds(0, h.PWM2_home); 108 | pwm_2.writeMicroseconds(1, h.PWM3_home); 109 | 110 | delay(3000); 111 | 112 | last_stamp = millis(); 113 | 114 | } 115 | 116 | void loop() { 117 | 118 | memcpy(sbus_ch, ch, sizeof(ch)); 119 | 120 | delay_time = map(sbus_ch[1], 1070, 1680, 30, 5); 121 | if (sbus_ch[1] >= 1070) { 122 | 123 | if ((sbus_ch[0] < 1048) && (sbus_ch[0] > 1000)) { 124 | ang_inc = 0; 125 | } else if (sbus_ch[0] >= 1048) { 126 | ang_inc = (int)map(sbus_ch[0], 1048, 1680, 11, 6); 127 | } else if (sbus_ch[0] <= 1000) { 128 | ang_inc = (int)map(sbus_ch[0], 368, 980, 6, 0); 129 | } 130 | delay_time = map(sbus_ch[1], 1070, 1680, 30, 5); 131 | 132 | } else if (sbus_ch[1] < 980) { 133 | if ((sbus_ch[0] < 1048) && (sbus_ch[0] > 1000)) { 134 | ang_inc = 6; 135 | } else if (sbus_ch[0] >= 1048) { 136 | ang_inc = (int)map(sbus_ch[0], 1048, 1680, 6, 11); 137 | } else if (sbus_ch[0] <= 1000) { 138 | ang_inc = (int)map(sbus_ch[0], 368, 980, 0, 6); 139 | } 140 | 141 | delay_time = map(sbus_ch[1], 980, 368, 30, 5); 142 | 143 | } else { 144 | ang_inc = 0; 145 | } 146 | 147 | 148 | Serial.print("sbus ch1 "); 149 | Serial.print(sbus_ch[0]); 150 | Serial.print(" sbus ch2 "); 151 | Serial.print(sbus_ch[1]); 152 | Serial.print(" ang_inc "); 153 | Serial.print(ang_inc); 154 | Serial.print(" counter "); 155 | Serial.print(counter); 156 | Serial.print(" push_period "); 157 | Serial.print(push_period); 158 | Serial.print(" delay time "); 159 | Serial.println(delay_time); 160 | 161 | 162 | 163 | // if (sbus_ch[1] >= 1070) { 164 | // 165 | // push_period = (millis() - last_push_thr_stamp); 166 | // 167 | // if (push_period > 500) { 168 | // throttle_trig = true; 169 | // } 170 | // 171 | // } else { 172 | // last_push_thr_stamp = millis(); 173 | // throttle_trig = false; 174 | // counter = 0; 175 | // push_period = 0; 176 | // } 177 | 178 | 179 | period = millis() - last_stamp; 180 | if ((period >= delay_time) && ((sbus_ch[1] >= 1070) || (sbus_ch[1] <= 980)) ) { 181 | 182 | if (counter < DATA_POINT_ALL) { 183 | 184 | crabWalking(ang_inc, counter); 185 | counter++; 186 | 187 | } else { 188 | 189 | counter = 0; 190 | } 191 | 192 | last_stamp = millis(); 193 | from_walk = true; 194 | 195 | 196 | } else if ((sbus_ch[1] < 1070) && (sbus_ch[1] > 980)) { 197 | 198 | if (from_walk) { 199 | setHomePosition(); 200 | } 201 | 202 | counter = 0; 203 | from_walk = false; 204 | delay(25); 205 | 206 | } else { 207 | // Do nothing... 208 | } 209 | 210 | 211 | Serial.println(); 212 | delay(1); 213 | } 214 | 215 | static void UART_ISR_ROUTINE(void *pvParameters) 216 | { 217 | uart_event_t event; 218 | size_t buffered_size; 219 | bool exit_condition = false; 220 | 221 | //Infinite loop to run main bulk of task 222 | while (1) { 223 | 224 | //Loop will continually block (i.e. wait) on event messages from the event queue 225 | if (xQueueReceive(uart2_queue, (void * )&event, (portTickType)portMAX_DELAY)) { 226 | 227 | //Handle received event 228 | if (event.type == UART_DATA) { 229 | 230 | uint8_t UART2_data[128]; 231 | //uint8_t buf[35]; 232 | //uint16_t ch[16]; 233 | int UART2_data_length = 0; 234 | ESP_ERROR_CHECK(uart_get_buffered_data_len(UART_NUM_2, (size_t*)&UART2_data_length)); 235 | UART2_data_length = uart_read_bytes(UART_NUM_2, UART2_data, UART2_data_length, 100); 236 | 237 | //Serial.println("LEN= ");Serial.println(UART2_data_length); 238 | 239 | //Serial.print("DATA= "); 240 | //for(byte i=0; i 2 | #include 3 | #include 4 | 5 | #define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150 6 | #define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600 7 | #define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates 8 | 9 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); 10 | Adafruit_PWMServoDriver pwm_2 = Adafruit_PWMServoDriver(0x41); 11 | 12 | HexapodLib h; 13 | int counter = 0; 14 | int ang_inc = 0; 15 | 16 | void setup() { 17 | Serial.begin(115200); 18 | 19 | pwm.begin(); 20 | pwm.setOscillatorFrequency(27000000); 21 | pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 22 | delay(5); 23 | 24 | pwm_2.begin(); 25 | pwm_2.setOscillatorFrequency(27000000); 26 | pwm_2.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates 27 | delay(5); 28 | 29 | Serial.println("******************************************************"); 30 | Serial.print("PWM1_home "); 31 | Serial.println(h.PWM1_home); 32 | Serial.print("PWM2_home "); 33 | Serial.println(h.PWM2_home); 34 | Serial.print("PWM3_home "); 35 | Serial.println(h.PWM3_home); 36 | 37 | // Serial.println("******************************************************"); 38 | // 39 | // Serial.println("X3"); 40 | // print_array(h.X3, DATA_POINT_ALL); 41 | // Serial.println("Y3"); 42 | // print_array(h.Y3, DATA_POINT_ALL); 43 | // Serial.println("Z3"); 44 | // print_array(h.Z3, DATA_POINT_ALL); 45 | // 46 | // Serial.println("******************************************************"); 47 | // 48 | // Serial.println("X4"); 49 | // print_array(h.X4, DATA_POINT_ALL); 50 | // Serial.println("Y4"); 51 | // print_array(h.Y4, DATA_POINT_ALL); 52 | // Serial.println("Z4"); 53 | // print_array(h.Z4, DATA_POINT_ALL); 54 | 55 | pwm.writeMicroseconds(0, h.PWM1_home); 56 | pwm.writeMicroseconds(1, h.PWM2_home); 57 | pwm.writeMicroseconds(2, h.PWM3_home); 58 | 59 | pwm.writeMicroseconds(3, h.PWM1_home); 60 | pwm.writeMicroseconds(4, h.PWM2_home); 61 | pwm.writeMicroseconds(5, h.PWM3_home); 62 | 63 | pwm.writeMicroseconds(6, h.PWM1_home); 64 | pwm.writeMicroseconds(7, h.PWM2_home); 65 | pwm.writeMicroseconds(8, h.PWM3_home); 66 | 67 | pwm.writeMicroseconds(9, h.PWM1_home); 68 | pwm.writeMicroseconds(10, h.PWM2_home); 69 | pwm.writeMicroseconds(11, h.PWM3_home); 70 | 71 | pwm.writeMicroseconds(12, h.PWM1_home); 72 | pwm.writeMicroseconds(13, h.PWM2_home); 73 | pwm.writeMicroseconds(14, h.PWM3_home); 74 | 75 | pwm.writeMicroseconds(15, h.PWM1_home); 76 | pwm_2.writeMicroseconds(0, h.PWM2_home); 77 | pwm_2.writeMicroseconds(1, h.PWM3_home); 78 | 79 | int pwm1_mid = (int)h.pwm1_mid; 80 | int pwm2_mid = (int)h.pwm2_mid; 81 | int pwm3_mid = (int)h.pwm3_mid; 82 | 83 | // pwm.writeMicroseconds(0, pwm1_mid); 84 | // pwm.writeMicroseconds(1, pwm2_mid); 85 | // pwm.writeMicroseconds(2, pwm3_mid); 86 | // 87 | // pwm.writeMicroseconds(3, pwm1_mid); 88 | // pwm.writeMicroseconds(4, pwm2_mid); 89 | // pwm.writeMicroseconds(5, pwm3_mid); 90 | // 91 | // pwm.writeMicroseconds(6, pwm1_mid); 92 | // pwm.writeMicroseconds(7, pwm2_mid); 93 | // pwm.writeMicroseconds(8, pwm3_mid); 94 | // 95 | // pwm.writeMicroseconds(9, pwm1_mid); 96 | // pwm.writeMicroseconds(10, pwm2_mid); 97 | // pwm.writeMicroseconds(11, pwm3_mid); 98 | // 99 | // pwm.writeMicroseconds(12, pwm1_mid); 100 | // pwm.writeMicroseconds(13, pwm2_mid); 101 | // pwm.writeMicroseconds(14, pwm3_mid); 102 | // 103 | // pwm.writeMicroseconds(15, pwm1_mid); 104 | // pwm_2.writeMicroseconds(0, pwm2_mid); 105 | // pwm_2.writeMicroseconds(1, pwm3_mid); 106 | 107 | delay(3000); 108 | } 109 | 110 | void loop() { 111 | if (counter < DATA_POINT_ALL) { 112 | 113 | pwm.writeMicroseconds(0, h.crab_walking_LUT[ang_inc][0][counter]); 114 | pwm.writeMicroseconds(1, h.crab_walking_LUT[ang_inc][1][counter]); 115 | pwm.writeMicroseconds(2, h.crab_walking_LUT[ang_inc][2][counter]); 116 | 117 | pwm.writeMicroseconds(3, h.crab_walking_LUT[ang_inc][3][counter]); 118 | pwm.writeMicroseconds(4, h.crab_walking_LUT[ang_inc][4][counter]); 119 | pwm.writeMicroseconds(5, h.crab_walking_LUT[ang_inc][5][counter]); 120 | 121 | pwm.writeMicroseconds(6, h.crab_walking_LUT[ang_inc][6][counter]); 122 | pwm.writeMicroseconds(7, h.crab_walking_LUT[ang_inc][7][counter]); 123 | pwm.writeMicroseconds(8, h.crab_walking_LUT[ang_inc][8][counter]); 124 | 125 | pwm.writeMicroseconds(9, h.crab_walking_LUT[ang_inc][9][counter]); 126 | pwm.writeMicroseconds(10, h.crab_walking_LUT[ang_inc][10][counter]); 127 | pwm.writeMicroseconds(11, h.crab_walking_LUT[ang_inc][11][counter]); 128 | 129 | pwm.writeMicroseconds(12, h.crab_walking_LUT[ang_inc][12][counter]); 130 | pwm.writeMicroseconds(13, h.crab_walking_LUT[ang_inc][13][counter]); 131 | pwm.writeMicroseconds(14, h.crab_walking_LUT[ang_inc][14][counter]); 132 | 133 | pwm.writeMicroseconds(15, h.crab_walking_LUT[ang_inc][15][counter]); 134 | pwm_2.writeMicroseconds(0, h.crab_walking_LUT[ang_inc][16][counter]); 135 | pwm_2.writeMicroseconds(1, h.crab_walking_LUT[ang_inc][17][counter]); 136 | 137 | 138 | counter++; 139 | 140 | } else { 141 | 142 | counter = 0; 143 | } 144 | 145 | delay(10); 146 | 147 | } 148 | 149 | void print_array(double* arr, int len) { 150 | 151 | for (int i = 0; i < len; i++) { 152 | Serial.print(arr[i]); 153 | Serial.print(" "); 154 | } 155 | Serial.println(" "); 156 | 157 | } 158 | 159 | 160 | void print_array(float* arr, int len) { 161 | 162 | for (int i = 0; i < len; i++) { 163 | Serial.print(arr[i]); 164 | Serial.print(" "); 165 | } 166 | Serial.println(" "); 167 | 168 | } 169 | 170 | void print_array(int* arr, int len) { 171 | 172 | for (int i = 0; i < len; i++) { 173 | Serial.print(arr[i]); 174 | Serial.print(" "); 175 | } 176 | Serial.println(" "); 177 | 178 | } 179 | --------------------------------------------------------------------------------