├── README.md ├── Raybot_test ├── include │ ├── README │ └── config.h ├── lib │ ├── ESP32-Arduino-CAN │ │ ├── LICENSE │ │ ├── README.md │ │ ├── examples │ │ │ ├── esp32can_basic │ │ │ │ └── esp32can_basic.ino │ │ │ ├── esp32can_filter │ │ │ │ └── esp32can_filter.ino │ │ │ └── esp32can_mirror │ │ │ │ └── esp32can_mirror.ino │ │ ├── keywords.txt │ │ ├── library.properties │ │ └── src │ │ │ ├── CAN.c │ │ │ ├── CAN.h │ │ │ ├── CAN_config.h │ │ │ ├── ESP32CAN.cpp │ │ │ ├── ESP32CAN.h │ │ │ └── can_regdef.h │ └── README ├── platformio.ini └── src │ ├── main.cpp │ ├── test.cpp │ └── test.h ├── URDF ├── CMakeLists.txt ├── config │ └── joint_names_URDF.yaml ├── export.log ├── launch │ ├── display.launch │ ├── display_humanoid.launch │ └── gazebo.launch ├── meshes │ ├── ai.STL │ ├── aia1.STL │ ├── aia2.STL │ ├── ail1.STL │ ├── ail2.STL │ ├── ail3.STL │ ├── ail4.STL │ ├── ail5.STL │ ├── ail6.STL │ ├── ail7.STL │ ├── ail8.STL │ ├── air1.STL │ ├── air2.STL │ ├── air3.STL │ ├── air4.STL │ ├── air5.STL │ ├── air6.STL │ ├── air7.STL │ ├── air8.STL │ ├── ait1.STL │ ├── ait2.STL │ └── ait3.STL ├── package.xml ├── rviz │ └── rviz.rviz └── urdf │ ├── URDF.csv │ ├── URDF.urdf │ └── raybotacan22.urdf ├── Webots └── translation_controller.py └── 中控.html /README.md: -------------------------------------------------------------------------------- 1 | # Raybot 2 | Raybot项目的代码 3 | 使用平台为Ros2-foxy与webots 4 | 包括webots控制代码，Raybot的urdf文件，以及esp部分的调试控制代码 5 | 视频链接：https://www.bilibili.com/video/BV17z421r7Wr/ 6 | 7 | 4.30：增加了调试文件，使用websocket与esp端进行连接 8 | -------------------------------------------------------------------------------- /Raybot_test/include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /Raybot_test/include/config.h: -------------------------------------------------------------------------------- 1 | #ifndef __CONFIG_H__ 2 | #define __CONFIG_H__ 3 | 4 | // PIN 5 | #define PIN_5V_EN 16 6 | 7 | #define CAN_TX_PIN 26 8 | #define CAN_RX_PIN 27 9 | #define CAN_SE_PIN 23 10 | 11 | #define RS485_EN_PIN 17 // 17 /RE 12 | #define RS485_TX_PIN 22 // 21 13 | #define RS485_RX_PIN 21 // 22 14 | #define RS485_SE_PIN 19 // 22 /SHDN 15 | 16 | #define SD_MISO_PIN 2 17 | #define SD_MOSI_PIN 15 18 | #define SD_SCLK_PIN 14 19 | #define SD_CS_PIN 13 20 | 21 | #define WS2812_PIN 4 22 | 23 | #endif -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Michael Wagner 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/README.md: -------------------------------------------------------------------------------- 1 | # Arduino Library for the ESP32 CAN Bus (ESP32-Arduino-CAN) 2 | 3 | ## Features 4 | 5 | * Support the CAN Bus from the ESP32 (SJA1000) 6 | * CAN Messages send and receive 7 | * Various Bus speeds 8 | * Standard and Extended Frames 9 | * CAN Message Filter 10 | 11 | 12 | 13 | ## Third Party Components 14 | 15 | - Arduino-ESP32-CAN-Demo 16 | - Arduino CAN Demo from [iotsharing.com - nhatuan84](https://github.com/nhatuan84/arduino-esp32-can-demo) 17 | 18 | - ESPCan Driver 19 | - Base CAN Driver from [Thomas Barth](https://github.com/ThomasBarth/ESP32-CAN-Driver) and [Nayar Systems](https://github.com/nayarsystems/ESP32-CAN-Driver) 20 | - General [Component CAN Driver Pack](https://github.com/ESP32DE/ESP32-CAN-Driver/tree/Component_CAN_Driver_Pack) Work for ESP-IDF with menuconfig from [rudi ;-)](http://esp32.de) 21 | 22 | ## Usage 23 | 24 | See the examples in the [/examples](examples) folder. -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/examples/esp32can_basic/esp32can_basic.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: your name 3 | * @Date: 2021-10-06 11:26:15 4 | * @LastEditTime: 2021-11-11 17:43:55 5 | * @LastEditors: Please set LastEditors 6 | * @Description: 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE 7 | * @FilePath: \ESP32-Arduino-CAN\examples\esp32can_basic\esp32can_basic.ino 8 | */ 9 | #include 10 | #include 11 | #include 12 | 13 | CAN_device_t CAN_cfg; // CAN Config 14 | unsigned long previousMillis = 0; // will store last time a CAN Message was send 15 | const int interval = 1000; // interval at which send CAN Messages (milliseconds) 16 | const int rx_queue_size = 10; // Receive Queue size 17 | 18 | void setup() 19 | { 20 | 21 | pinMode(16, OUTPUT); 22 | digitalWrite(16, 1); 23 | 24 | pinMode(23, OUTPUT); 25 | digitalWrite(23, 0); 26 | 27 | Serial.begin(115200); 28 | Serial.println("Basic Demo - ESP32-Arduino-CAN"); 29 | CAN_cfg.speed = CAN_SPEED_1000KBPS; 30 | CAN_cfg.tx_pin_id = GPIO_NUM_27; 31 | CAN_cfg.rx_pin_id = GPIO_NUM_26; 32 | CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t)); 33 | // Init CAN Module 34 | ESP32Can.CANInit(); 35 | 36 | Serial.print("CAN SPEED :"); 37 | Serial.println(CAN_cfg.speed); 38 | } 39 | 40 | void loop() 41 | { 42 | 43 | CAN_frame_t rx_frame; 44 | 45 | unsigned long currentMillis = millis(); 46 | 47 | // Receive next CAN frame from queue 48 | if (xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS) == pdTRUE) 49 | { 50 | 51 | if (rx_frame.FIR.B.FF == CAN_frame_std) 52 | { 53 | printf("New standard frame"); 54 | } 55 | else 56 | { 57 | printf("New extended frame"); 58 | } 59 | 60 | if (rx_frame.FIR.B.RTR == CAN_RTR) 61 | { 62 | printf(" RTR from 0x%08X, DLC %d\r\n", rx_frame.MsgID, rx_frame.FIR.B.DLC); 63 | } 64 | else 65 | { 66 | printf(" from 0x%08X, DLC %d, Data ", rx_frame.MsgID, rx_frame.FIR.B.DLC); 67 | for (int i = 0; i < rx_frame.FIR.B.DLC; i++) 68 | { 69 | printf("0x%02X ", rx_frame.data.u8[i]); 70 | } 71 | printf("\n"); 72 | } 73 | } 74 | // Send CAN Message 75 | if (currentMillis - previousMillis >= interval) 76 | { 77 | previousMillis = currentMillis; 78 | CAN_frame_t tx_frame; 79 | tx_frame.FIR.B.FF = CAN_frame_std; 80 | tx_frame.MsgID = 0x001; 81 | tx_frame.FIR.B.DLC = 8; 82 | tx_frame.data.u8[0] = 0x00; 83 | tx_frame.data.u8[1] = 0x01; 84 | tx_frame.data.u8[2] = 0x02; 85 | tx_frame.data.u8[3] = 0x03; 86 | tx_frame.data.u8[4] = 0x04; 87 | tx_frame.data.u8[5] = 0x05; 88 | tx_frame.data.u8[6] = 0x06; 89 | tx_frame.data.u8[7] = 0x07; 90 | 91 | ESP32Can.CANWriteFrame(&tx_frame); 92 | Serial.println("CAN"); 93 | } 94 | } 95 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/examples/esp32can_filter/esp32can_filter.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | CAN_device_t CAN_cfg; // CAN Config 6 | const int rx_queue_size = 10; // Receive Queue size 7 | 8 | void setup() { 9 | Serial.begin(115200); 10 | Serial.println("Filter Demo - ESP32-Arduino-CAN"); 11 | CAN_cfg.speed = CAN_SPEED_125KBPS; 12 | CAN_cfg.tx_pin_id = GPIO_NUM_5; 13 | CAN_cfg.rx_pin_id = GPIO_NUM_4; 14 | CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t)); 15 | 16 | // Set CAN Filter 17 | // See in the SJA1000 Datasheet chapter "6.4.15 Acceptance filter" 18 | // and the APPLICATION NOTE AN97076 chapter "4.1.2 Acceptance Filter" 19 | // for PeliCAN Mode 20 | CAN_filter_t p_filter; 21 | p_filter.FM = Single_Mode; 22 | 23 | p_filter.ACR0 = 0x29; 24 | p_filter.ACR1 = 0; 25 | p_filter.ACR2 = 0; 26 | p_filter.ACR3 = 0; 27 | 28 | p_filter.AMR0 = 0; 29 | p_filter.AMR1 = 0xFF; 30 | p_filter.AMR2 = 0xFF; 31 | p_filter.AMR3 = 0xFF; 32 | ESP32Can.CANConfigFilter(&p_filter); 33 | 34 | // Init CAN Module 35 | ESP32Can.CANInit(); 36 | } 37 | 38 | void loop() { 39 | 40 | CAN_frame_t rx_frame; 41 | 42 | unsigned long currentMillis = millis(); 43 | 44 | // Receive next CAN frame from queue 45 | if (xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS) == pdTRUE) { 46 | 47 | if (rx_frame.FIR.B.FF == CAN_frame_std) { 48 | printf("New standard frame"); 49 | } 50 | else { 51 | printf("New extended frame"); 52 | } 53 | 54 | if (rx_frame.FIR.B.RTR == CAN_RTR) { 55 | printf(" RTR from 0x%08X, DLC %d\r\n", rx_frame.MsgID, rx_frame.FIR.B.DLC); 56 | } 57 | else { 58 | printf(" from 0x%08X, DLC %d, Data ", rx_frame.MsgID, rx_frame.FIR.B.DLC); 59 | for (int i = 0; i < rx_frame.FIR.B.DLC; i++) { 60 | printf("0x%02X ", rx_frame.data.u8[i]); 61 | } 62 | printf("\n"); 63 | } 64 | } 65 | } 66 | 67 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/examples/esp32can_mirror/esp32can_mirror.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | CAN_device_t CAN_cfg; // CAN Config 6 | const int rx_queue_size = 10; // Receive Queue size 7 | 8 | void setup() { 9 | Serial.begin(115200); 10 | Serial.println("Mirror Demo - ESP32-Arduino-CAN"); 11 | CAN_cfg.speed = CAN_SPEED_125KBPS; 12 | CAN_cfg.tx_pin_id = GPIO_NUM_26; 13 | CAN_cfg.rx_pin_id = GPIO_NUM_27; 14 | CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t)); 15 | // Init CAN Module 16 | ESP32Can.CANInit(); 17 | } 18 | 19 | void loop() { 20 | CAN_frame_t rx_frame; 21 | //receive next CAN frame from queue 22 | if (xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS) == pdTRUE) { 23 | 24 | if (rx_frame.FIR.B.FF == CAN_frame_std) { 25 | printf("New standard frame"); 26 | } 27 | else { 28 | printf("New extended frame"); 29 | } 30 | 31 | if (rx_frame.FIR.B.RTR == CAN_RTR) { 32 | printf(" RTR from 0x%08X, DLC %d\r\n", rx_frame.MsgID, rx_frame.FIR.B.DLC); 33 | } 34 | else { 35 | printf(" from 0x%08X, DLC %d, Data ", rx_frame.MsgID, rx_frame.FIR.B.DLC); 36 | for (int i = 0; i < rx_frame.FIR.B.DLC; i++) { 37 | printf("0x%02X ", rx_frame.data.u8[i]); 38 | } 39 | printf("\n"); 40 | } 41 | 42 | //respond to sender 43 | ESP32Can.CANWriteFrame(&rx_frame); 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map ESP32CAN 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | ESP32CAN KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | CANInit KEYWORD2 15 | CANWriteFrame KEYWORD2 16 | CANStop KEYWORD2 17 | CANConfigFilter KEYWORD2 18 | ####################################### 19 | # Constants (LITERAL1) 20 | ####################################### 21 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/library.properties: -------------------------------------------------------------------------------- 1 | name=ESP32CAN 2 | version=0.0.1 3 | author=Michael Wagner 4 | maintainer=https://github.com/miwagner 5 | sentence=ESP32-Arduino-CAN 6 | paragraph=ESP32-Arduino-CAN 7 | category=Device Control 8 | url=https://github.com/miwagner/ESP32-Arduino-CAN 9 | architectures=esp32 10 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/src/CAN.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @section License 3 | * 4 | * The MIT License (MIT) 5 | * 6 | * Copyright (c) 2017, Thomas Barth, barth-dev.de 7 | * 2017, Jaime Breva, jbreva@nayarsystems.com 8 | * 2018, Michael Wagner, mw@iot-make.de 9 | * 10 | * Permission is hereby granted, free of charge, to any person 11 | * obtaining a copy of this software and associated documentation 12 | * files (the "Software"), to deal in the Software without 13 | * restriction, including without limitation the rights to use, copy, 14 | * modify, merge, publish, distribute, sublicense, and/or sell copies 15 | * of the Software, and to permit persons to whom the Software is 16 | * furnished to do so, subject to the following conditions: 17 | * 18 | * The above copyright notice and this permission notice shall be 19 | * included in all copies or substantial portions of the Software. 20 | * 21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 22 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 23 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 24 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 25 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 26 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 27 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 28 | * SOFTWARE. 29 | * 30 | */ 31 | 32 | #include "CAN.h" 33 | 34 | #include "freertos/FreeRTOS.h" 35 | #include "freertos/queue.h" 36 | 37 | #include "esp_intr.h" 38 | #include "soc/dport_reg.h" 39 | #include 40 | 41 | #include "driver/gpio.h" 42 | 43 | #include "can_regdef.h" 44 | #include "CAN_config.h" 45 | 46 | // CAN Filter - no acceptance filter 47 | static CAN_filter_t __filter = { Dual_Mode, 0, 0, 0, 0, 0Xff, 0Xff, 0Xff, 0Xff }; 48 | 49 | static void CAN_read_frame_phy(); 50 | static void CAN_isr(void *arg_p); 51 | static int CAN_write_frame_phy(const CAN_frame_t *p_frame); 52 | static SemaphoreHandle_t sem_tx_complete; 53 | 54 | static void CAN_isr(void *arg_p) { 55 | 56 | // Interrupt flag buffer 57 | __CAN_IRQ_t interrupt; 58 | BaseType_t higherPriorityTaskWoken = pdFALSE; 59 | 60 | // Read interrupt status and clear flags 61 | interrupt = MODULE_CAN->IR.U; 62 | 63 | // Handle RX frame available interrupt 64 | if ((interrupt & __CAN_IRQ_RX) != 0) 65 | CAN_read_frame_phy(&higherPriorityTaskWoken); 66 | 67 | // Handle TX complete interrupt 68 | // Handle error interrupts. 69 | if ((interrupt & (__CAN_IRQ_TX | __CAN_IRQ_ERR //0x4 70 | | __CAN_IRQ_DATA_OVERRUN // 0x8 71 | | __CAN_IRQ_WAKEUP // 0x10 72 | | __CAN_IRQ_ERR_PASSIVE // 0x20 73 | | __CAN_IRQ_ARB_LOST // 0x40 74 | | __CAN_IRQ_BUS_ERR // 0x80 75 | )) != 0) { 76 | xSemaphoreGiveFromISR(sem_tx_complete, &higherPriorityTaskWoken); 77 | } 78 | 79 | // check if any higher priority task has been woken by any handler 80 | if (higherPriorityTaskWoken) 81 | portYIELD_FROM_ISR(); 82 | } 83 | 84 | static void CAN_read_frame_phy(BaseType_t *higherPriorityTaskWoken) { 85 | 86 | // byte iterator 87 | uint8_t __byte_i; 88 | 89 | // frame read buffer 90 | CAN_frame_t __frame; 91 | 92 | // check if we have a queue. If not, operation is aborted. 93 | if (CAN_cfg.rx_queue == NULL) { 94 | // Let the hardware know the frame has been read. 95 | MODULE_CAN->CMR.B.RRB = 1; 96 | return; 97 | } 98 | 99 | // get FIR 100 | __frame.FIR.U = MODULE_CAN->MBX_CTRL.FCTRL.FIR.U; 101 | 102 | // check if this is a standard or extended CAN frame 103 | // standard frame 104 | if (__frame.FIR.B.FF == CAN_frame_std) { 105 | 106 | // Get Message ID 107 | __frame.MsgID = _CAN_GET_STD_ID; 108 | 109 | // deep copy data bytes 110 | for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++) 111 | __frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i]; 112 | } 113 | // extended frame 114 | else { 115 | 116 | // Get Message ID 117 | __frame.MsgID = _CAN_GET_EXT_ID; 118 | 119 | // deep copy data bytes 120 | for (__byte_i = 0; __byte_i < __frame.FIR.B.DLC; __byte_i++) 121 | __frame.data.u8[__byte_i] = MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i]; 122 | } 123 | 124 | // send frame to input queue 125 | xQueueSendToBackFromISR(CAN_cfg.rx_queue, &__frame, higherPriorityTaskWoken); 126 | 127 | // Let the hardware know the frame has been read. 128 | MODULE_CAN->CMR.B.RRB = 1; 129 | } 130 | 131 | static int CAN_write_frame_phy(const CAN_frame_t *p_frame) { 132 | 133 | // byte iterator 134 | uint8_t __byte_i; 135 | 136 | // copy frame information record 137 | MODULE_CAN->MBX_CTRL.FCTRL.FIR.U = p_frame->FIR.U; 138 | 139 | // standard frame 140 | if (p_frame->FIR.B.FF == CAN_frame_std) { 141 | 142 | // Write message ID 143 | _CAN_SET_STD_ID(p_frame->MsgID); 144 | 145 | // Copy the frame data to the hardware 146 | for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++) 147 | MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.data[__byte_i] = p_frame->data.u8[__byte_i]; 148 | 149 | } 150 | // extended frame 151 | else { 152 | 153 | // Write message ID 154 | _CAN_SET_EXT_ID(p_frame->MsgID); 155 | 156 | // Copy the frame data to the hardware 157 | for (__byte_i = 0; __byte_i < p_frame->FIR.B.DLC; __byte_i++) 158 | MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.data[__byte_i] = p_frame->data.u8[__byte_i]; 159 | } 160 | 161 | // Transmit frame 162 | MODULE_CAN->CMR.B.TR = 1; 163 | 164 | return 0; 165 | } 166 | 167 | int CAN_init() { 168 | 169 | // Time quantum 170 | double __tq; 171 | 172 | // enable module 173 | DPORT_SET_PERI_REG_MASK(DPORT_PERIP_CLK_EN_REG, DPORT_CAN_CLK_EN); 174 | DPORT_CLEAR_PERI_REG_MASK(DPORT_PERIP_RST_EN_REG, DPORT_CAN_RST); 175 | 176 | // configure TX pin 177 | gpio_set_level(CAN_cfg.tx_pin_id, 1); 178 | gpio_set_direction(CAN_cfg.tx_pin_id, GPIO_MODE_OUTPUT); 179 | gpio_matrix_out(CAN_cfg.tx_pin_id, CAN_TX_IDX, 0, 0); 180 | gpio_pad_select_gpio(CAN_cfg.tx_pin_id); 181 | 182 | // configure RX pin 183 | gpio_set_direction(CAN_cfg.rx_pin_id, GPIO_MODE_INPUT); 184 | gpio_matrix_in(CAN_cfg.rx_pin_id, CAN_RX_IDX, 0); 185 | gpio_pad_select_gpio(CAN_cfg.rx_pin_id); 186 | 187 | // set to PELICAN mode 188 | MODULE_CAN->CDR.B.CAN_M = 0x1; 189 | 190 | // synchronization jump width is the same for all baud rates 191 | MODULE_CAN->BTR0.B.SJW = 0x1; 192 | 193 | // TSEG2 is the same for all baud rates 194 | MODULE_CAN->BTR1.B.TSEG2 = 0x1; 195 | 196 | // select time quantum and set TSEG1 197 | switch (CAN_cfg.speed) { 198 | case CAN_SPEED_1000KBPS: 199 | MODULE_CAN->BTR1.B.TSEG1 = 0x4; 200 | __tq = 0.125; 201 | break; 202 | 203 | case CAN_SPEED_800KBPS: 204 | MODULE_CAN->BTR1.B.TSEG1 = 0x6; 205 | __tq = 0.125; 206 | break; 207 | 208 | case CAN_SPEED_200KBPS: 209 | MODULE_CAN->BTR1.B.TSEG1 = 0xc; 210 | MODULE_CAN->BTR1.B.TSEG2 = 0x5; 211 | __tq = 0.25; 212 | break; 213 | 214 | default: 215 | MODULE_CAN->BTR1.B.TSEG1 = 0xc; 216 | __tq = ((float) 1000 / CAN_cfg.speed) / 16; 217 | } 218 | 219 | // set baud rate prescaler 220 | MODULE_CAN->BTR0.B.BRP = (uint8_t) round((((APB_CLK_FREQ * __tq) / 2) - 1) / 1000000) - 1; 221 | 222 | /* Set sampling 223 | * 1 -> triple; the bus is sampled three times; recommended for low/medium speed buses (class A and B) where 224 | * filtering spikes on the bus line is beneficial 0 -> single; the bus is sampled once; recommended for high speed 225 | * buses (SAE class C)*/ 226 | MODULE_CAN->BTR1.B.SAM = 0x1; 227 | 228 | // enable all interrupts 229 | MODULE_CAN->IER.U = 0xef; //ESP32 V3 0XEF ESP32 NOT V3 0XFF 230 | 231 | // Set acceptance filter 232 | MODULE_CAN->MOD.B.AFM = __filter.FM; 233 | MODULE_CAN->MBX_CTRL.ACC.CODE[0] = __filter.ACR0; 234 | MODULE_CAN->MBX_CTRL.ACC.CODE[1] = __filter.ACR1; 235 | MODULE_CAN->MBX_CTRL.ACC.CODE[2] = __filter.ACR2; 236 | MODULE_CAN->MBX_CTRL.ACC.CODE[3] = __filter.ACR3; 237 | MODULE_CAN->MBX_CTRL.ACC.MASK[0] = __filter.AMR0; 238 | MODULE_CAN->MBX_CTRL.ACC.MASK[1] = __filter.AMR1; 239 | MODULE_CAN->MBX_CTRL.ACC.MASK[2] = __filter.AMR2; 240 | MODULE_CAN->MBX_CTRL.ACC.MASK[3] = __filter.AMR3; 241 | 242 | // set to normal mode 243 | MODULE_CAN->OCR.B.OCMODE = __CAN_OC_NOM; 244 | 245 | // clear error counters 246 | MODULE_CAN->TXERR.U = 0; 247 | MODULE_CAN->RXERR.U = 0; 248 | (void) MODULE_CAN->ECC; 249 | 250 | // clear interrupt flags 251 | (void) MODULE_CAN->IR.U; 252 | 253 | // install CAN ISR 254 | esp_intr_alloc(ETS_CAN_INTR_SOURCE, 0, CAN_isr, NULL, NULL); 255 | 256 | // allocate the tx complete semaphore 257 | sem_tx_complete = xSemaphoreCreateBinary(); 258 | 259 | // Showtime. Release Reset Mode. 260 | MODULE_CAN->MOD.B.RM = 0; 261 | 262 | return 0; 263 | } 264 | 265 | int CAN_write_frame(const CAN_frame_t *p_frame) { 266 | if (sem_tx_complete == NULL) { 267 | return -1; 268 | } 269 | 270 | // Write the frame to the controller 271 | CAN_write_frame_phy(p_frame); 272 | 273 | // wait for the frame tx to complete 274 | xSemaphoreTake(sem_tx_complete, portMAX_DELAY); 275 | 276 | return 0; 277 | } 278 | 279 | int CAN_stop() { 280 | // enter reset mode 281 | MODULE_CAN->MOD.B.RM = 1; 282 | 283 | return 0; 284 | } 285 | 286 | int CAN_config_filter(const CAN_filter_t* p_filter) { 287 | 288 | __filter.FM = p_filter->FM; 289 | __filter.ACR0 = p_filter->ACR0; 290 | __filter.ACR1 = p_filter->ACR1; 291 | __filter.ACR2 = p_filter->ACR2; 292 | __filter.ACR3 = p_filter->ACR3; 293 | __filter.AMR0 = p_filter->AMR0; 294 | __filter.AMR1 = p_filter->AMR1; 295 | __filter.AMR2 = p_filter->AMR2; 296 | __filter.AMR3 = p_filter->AMR3; 297 | 298 | return 0; 299 | } -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/src/CAN.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @section License 3 | * 4 | * The MIT License (MIT) 5 | * 6 | * Copyright (c) 2017, Thomas Barth, barth-dev.de 7 | * 2018, Michael Wagner, mw@iot-make.de 8 | * Permission is hereby granted, free of charge, to any person 9 | * obtaining a copy of this software and associated documentation 10 | * files (the "Software"), to deal in the Software without 11 | * restriction, including without limitation the rights to use, copy, 12 | * modify, merge, publish, distribute, sublicense, and/or sell copies 13 | * of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be 17 | * included in all copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 20 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 21 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 22 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 23 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 24 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 25 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 26 | * SOFTWARE. 27 | */ 28 | 29 | #ifndef __DRIVERS_CAN_H__ 30 | #define __DRIVERS_CAN_H__ 31 | 32 | #include 33 | #include "CAN_config.h" 34 | 35 | #ifdef __cplusplus 36 | extern "C" { 37 | #endif 38 | 39 | /** 40 | * \brief CAN frame type (standard/extended) 41 | */ 42 | typedef enum { 43 | CAN_frame_std = 0, /**< Standard frame, using 11 bit identifer. */ 44 | CAN_frame_ext = 1 /**< Extended frame, using 29 bit identifer. */ 45 | } CAN_frame_format_t; 46 | 47 | /** 48 | * \brief CAN RTR 49 | */ 50 | typedef enum { 51 | CAN_no_RTR = 0, /**< No RTR frame. */ 52 | CAN_RTR = 1 /**< RTR frame. */ 53 | } CAN_RTR_t; 54 | 55 | /** \brief Frame information record type */ 56 | typedef union { 57 | uint32_t U; /**< \brief Unsigned access */ 58 | struct { 59 | uint8_t DLC : 4; /**< \brief [3:0] DLC, Data length container */ 60 | unsigned int unknown_2 : 2; /**< \brief \internal unknown */ 61 | CAN_RTR_t RTR : 1; /**< \brief [6:6] RTR, Remote Transmission Request */ 62 | CAN_frame_format_t FF : 1; /**< \brief [7:7] Frame Format, see# CAN_frame_format_t*/ 63 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 64 | } B; 65 | } CAN_FIR_t; 66 | 67 | /** \brief CAN Frame structure */ 68 | typedef struct { 69 | CAN_FIR_t FIR; /**< \brief Frame information record*/ 70 | uint32_t MsgID; /**< \brief Message ID */ 71 | union { 72 | uint8_t u8[8]; /**< \brief Payload byte access*/ 73 | uint32_t u32[2]; /**< \brief Payload u32 access*/ 74 | uint64_t u64; /**< \brief Payload u64 access*/ 75 | } data; 76 | } CAN_frame_t; 77 | 78 | typedef enum { 79 | Dual_Mode=0, /**< \brief The dual acceptance filter option is enabled (two filters, each with the length of 16 bit are active) */ 80 | Single_Mode=1 /**< \brief The single acceptance filter option is enabled (one filter with the length of 32 bit is active) */ 81 | } CAN_filter_mode_t; 82 | 83 | /** \brief CAN Filter structure */ 84 | typedef struct { 85 | CAN_filter_mode_t FM:1; /**< \brief [0:0] Filter Mode */ 86 | uint8_t ACR0; /**< \brief Acceptance Code Register ACR0 */ 87 | uint8_t ACR1; /**< \brief Acceptance Code Register ACR1 */ 88 | uint8_t ACR2; /**< \brief Acceptance Code Register ACR2 */ 89 | uint8_t ACR3; /**< \brief Acceptance Code Register ACR3 */ 90 | uint8_t AMR0; /**< \brief Acceptance Mask Register AMR0 */ 91 | uint8_t AMR1; /**< \brief Acceptance Mask Register AMR1 */ 92 | uint8_t AMR2; /**< \brief Acceptance Mask Register AMR2 */ 93 | uint8_t AMR3; /**< \brief Acceptance Mask Register AMR3 */ 94 | } CAN_filter_t; 95 | 96 | /** 97 | * \brief Initialize the CAN Module 98 | * 99 | * \return 0 CAN Module had been initialized 100 | */ 101 | int CAN_init(void); 102 | 103 | /** 104 | * \brief Send a can frame 105 | * 106 | * \param p_frame Pointer to the frame to be send, see #CAN_frame_t 107 | * \return 0 Frame has been written to the module 108 | */ 109 | int CAN_write_frame(const CAN_frame_t *p_frame); 110 | 111 | /** 112 | * \brief Stops the CAN Module 113 | * 114 | * \return 0 CAN Module was stopped 115 | */ 116 | int CAN_stop(void); 117 | 118 | /** 119 | * \brief Config CAN Filter, must call before CANInit() 120 | * 121 | * \param p_filter Pointer to the filter, see #CAN_filter_t 122 | * \return 0 CAN Filter had been initialized 123 | */ 124 | int CAN_config_filter(const CAN_filter_t* p_filter); 125 | 126 | 127 | #ifdef __cplusplus 128 | } 129 | #endif 130 | 131 | #endif 132 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/src/CAN_config.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @section License 3 | * 4 | * The MIT License (MIT) 5 | * 6 | * Copyright (c) 2017, Thomas Barth, barth-dev.de 7 | * 8 | * Permission is hereby granted, free of charge, to any person 9 | * obtaining a copy of this software and associated documentation 10 | * files (the "Software"), to deal in the Software without 11 | * restriction, including without limitation the rights to use, copy, 12 | * modify, merge, publish, distribute, sublicense, and/or sell copies 13 | * of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be 17 | * included in all copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 20 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 21 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 22 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 23 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 24 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 25 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 26 | * SOFTWARE. 27 | */ 28 | 29 | #ifndef __DRIVERS_CAN_CFG_H__ 30 | #define __DRIVERS_CAN_CFG_H__ 31 | 32 | #include "freertos/FreeRTOS.h" 33 | #include "freertos/queue.h" 34 | #include "freertos/task.h" 35 | #include "driver/gpio.h" 36 | #include "freertos/semphr.h" 37 | 38 | #ifdef __cplusplus 39 | extern "C" { 40 | #endif 41 | 42 | /** \brief CAN Node Bus speed */ 43 | typedef enum { 44 | CAN_SPEED_100KBPS = 100, /**< \brief CAN Node runs at 100kBit/s. */ 45 | CAN_SPEED_125KBPS = 125, /**< \brief CAN Node runs at 125kBit/s. */ 46 | CAN_SPEED_200KBPS = 200, /**< \brief CAN Node runs at 250kBit/s. */ 47 | CAN_SPEED_250KBPS = 250, /**< \brief CAN Node runs at 250kBit/s. */ 48 | CAN_SPEED_500KBPS = 500, /**< \brief CAN Node runs at 500kBit/s. */ 49 | CAN_SPEED_800KBPS = 800, /**< \brief CAN Node runs at 800kBit/s. */ 50 | CAN_SPEED_1000KBPS = 1000 /**< \brief CAN Node runs at 1000kBit/s. */ 51 | } CAN_speed_t; 52 | 53 | /** \brief CAN configuration structure */ 54 | typedef struct { 55 | CAN_speed_t speed; /**< \brief CAN speed. */ 56 | gpio_num_t tx_pin_id; /**< \brief TX pin. */ 57 | gpio_num_t rx_pin_id; /**< \brief RX pin. */ 58 | QueueHandle_t rx_queue; /**< \brief Handler to FreeRTOS RX queue. */ 59 | QueueHandle_t tx_queue; /**< \brief Handler to FreeRTOS TX queue. */ 60 | TaskHandle_t tx_handle; /**< \brief Handler to FreeRTOS TX task. */ 61 | TaskHandle_t rx_handle; /**< \brief Handler to FreeRTOS RX task. */ 62 | } CAN_device_t; 63 | 64 | /** \brief CAN configuration reference */ 65 | extern CAN_device_t CAN_cfg; 66 | 67 | #ifdef __cplusplus 68 | } 69 | #endif 70 | 71 | #endif /* __DRIVERS_CAN_CFG_H__ */ 72 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/src/ESP32CAN.cpp: -------------------------------------------------------------------------------- 1 | #include "ESP32CAN.h" 2 | 3 | int ESP32CAN::CANInit() 4 | { 5 | return CAN_init(); 6 | } 7 | int ESP32CAN::CANWriteFrame(const CAN_frame_t* p_frame) 8 | { 9 | return CAN_write_frame(p_frame); 10 | } 11 | int ESP32CAN::CANStop() 12 | { 13 | return CAN_stop(); 14 | } 15 | int ESP32CAN::CANConfigFilter(const CAN_filter_t* p_filter) 16 | { 17 | return CAN_config_filter(p_filter); 18 | } 19 | 20 | ESP32CAN ESP32Can; 21 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/src/ESP32CAN.h: -------------------------------------------------------------------------------- 1 | #ifndef ESP32CAN_H 2 | #define ESP32CAN_H 3 | 4 | #include "CAN_config.h" 5 | #include "CAN.h" 6 | 7 | class ESP32CAN 8 | { 9 | public: 10 | int CANInit(); 11 | int CANConfigFilter(const CAN_filter_t* p_filter); 12 | int CANWriteFrame(const CAN_frame_t* p_frame); 13 | int CANStop(); 14 | }; 15 | 16 | extern ESP32CAN ESP32Can; 17 | #endif 18 | -------------------------------------------------------------------------------- /Raybot_test/lib/ESP32-Arduino-CAN/src/can_regdef.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @section License 3 | * 4 | * The MIT License (MIT) 5 | * 6 | * Copyright (c) 2017, Thomas Barth, barth-dev.de 7 | * 8 | * Permission is hereby granted, free of charge, to any person 9 | * obtaining a copy of this software and associated documentation 10 | * files (the "Software"), to deal in the Software without 11 | * restriction, including without limitation the rights to use, copy, 12 | * modify, merge, publish, distribute, sublicense, and/or sell copies 13 | * of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be 17 | * included in all copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 20 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 21 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 22 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 23 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 24 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 25 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 26 | * SOFTWARE. 27 | */ 28 | 29 | #ifndef __DRIVERS_CAN_REGDEF_H_ 30 | #define __DRIVERS_CAN_REGDEF_H_ 31 | 32 | #include "CAN.h" //CAN_FIR_t 33 | 34 | #ifdef __cplusplus 35 | extern "C" { 36 | #endif 37 | 38 | /** \brief Start address of CAN registers */ 39 | #define MODULE_CAN ((volatile CAN_Module_t *) 0x3ff6b000) 40 | 41 | /** \brief Get standard message ID */ 42 | #define _CAN_GET_STD_ID \ 43 | (((uint32_t) MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.ID[0] << 3) | (MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.ID[1] >> 5)) 44 | 45 | /** \brief Get extended message ID */ 46 | #define _CAN_GET_EXT_ID \ 47 | (((uint32_t) MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[0] << 21) | \ 48 | (MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[1] << 13) | (MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[2] << 5) | \ 49 | (MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[3] >> 3)) 50 | 51 | /** \brief Set standard message ID */ 52 | #define _CAN_SET_STD_ID(x) \ 53 | MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.ID[0] = ((x) >> 3); \ 54 | MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.STD.ID[1] = ((x) << 5); 55 | 56 | /** \brief Set extended message ID */ 57 | #define _CAN_SET_EXT_ID(x) \ 58 | MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[0] = ((x) >> 21); \ 59 | MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[1] = ((x) >> 13); \ 60 | MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[2] = ((x) >> 5); \ 61 | MODULE_CAN->MBX_CTRL.FCTRL.TX_RX.EXT.ID[3] = ((x) << 3); 62 | 63 | /** \brief Interrupt status register */ 64 | typedef enum { 65 | __CAN_IRQ_RX = BIT(0), /**< \brief RX Interrupt */ 66 | __CAN_IRQ_TX = BIT(1), /**< \brief TX Interrupt */ 67 | __CAN_IRQ_ERR = BIT(2), /**< \brief Error Interrupt */ 68 | __CAN_IRQ_DATA_OVERRUN = BIT(3), /**< \brief Data Overrun Interrupt */ 69 | __CAN_IRQ_WAKEUP = BIT(4), /**< \brief Wakeup Interrupt */ 70 | __CAN_IRQ_ERR_PASSIVE = BIT(5), /**< \brief Passive Error Interrupt */ 71 | __CAN_IRQ_ARB_LOST = BIT(6), /**< \brief Arbitration lost interrupt */ 72 | __CAN_IRQ_BUS_ERR = BIT(7), /**< \brief Bus error Interrupt */ 73 | } __CAN_IRQ_t; 74 | 75 | /** \brief OCMODE options. */ 76 | typedef enum { 77 | __CAN_OC_BOM = 0b00, /**< \brief bi-phase output mode */ 78 | __CAN_OC_TOM = 0b01, /**< \brief test output mode */ 79 | __CAN_OC_NOM = 0b10, /**< \brief normal output mode */ 80 | __CAN_OC_COM = 0b11, /**< \brief clock output mode */ 81 | } __CAN_OCMODE_t; 82 | 83 | /** 84 | * CAN controller (SJA1000). 85 | */ 86 | typedef struct { 87 | union { 88 | uint32_t U; /**< \brief Unsigned access */ 89 | struct { 90 | unsigned int RM : 1; /**< \brief MOD.0 Reset Mode */ 91 | unsigned int LOM : 1; /**< \brief MOD.1 Listen Only Mode */ 92 | unsigned int STM : 1; /**< \brief MOD.2 Self Test Mode */ 93 | unsigned int AFM : 1; /**< \brief MOD.3 Acceptance Filter Mode */ 94 | unsigned int SM : 1; /**< \brief MOD.4 Sleep Mode */ 95 | unsigned int reserved_27 : 27; /**< \brief \internal Reserved */ 96 | } B; 97 | } MOD; 98 | union { 99 | uint32_t U; /**< \brief Unsigned access */ 100 | struct { 101 | unsigned int TR : 1; /**< \brief CMR.0 Transmission Request */ 102 | unsigned int AT : 1; /**< \brief CMR.1 Abort Transmission */ 103 | unsigned int RRB : 1; /**< \brief CMR.2 Release Receive Buffer */ 104 | unsigned int CDO : 1; /**< \brief CMR.3 Clear Data Overrun */ 105 | unsigned int GTS : 1; /**< \brief CMR.4 Go To Sleep */ 106 | unsigned int reserved_27 : 27; /**< \brief \internal Reserved */ 107 | } B; 108 | } CMR; 109 | union { 110 | uint32_t U; /**< \brief Unsigned access */ 111 | struct { 112 | unsigned int RBS : 1; /**< \brief SR.0 Receive Buffer Status */ 113 | unsigned int DOS : 1; /**< \brief SR.1 Data Overrun Status */ 114 | unsigned int TBS : 1; /**< \brief SR.2 Transmit Buffer Status */ 115 | unsigned int TCS : 1; /**< \brief SR.3 Transmission Complete Status */ 116 | unsigned int RS : 1; /**< \brief SR.4 Receive Status */ 117 | unsigned int TS : 1; /**< \brief SR.5 Transmit Status */ 118 | unsigned int ES : 1; /**< \brief SR.6 Error Status */ 119 | unsigned int BS : 1; /**< \brief SR.7 Bus Status */ 120 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 121 | } B; 122 | } SR; 123 | union { 124 | uint32_t U; /**< \brief Unsigned access */ 125 | struct { 126 | unsigned int RI : 1; /**< \brief IR.0 Receive Interrupt */ 127 | unsigned int TI : 1; /**< \brief IR.1 Transmit Interrupt */ 128 | unsigned int EI : 1; /**< \brief IR.2 Error Interrupt */ 129 | unsigned int DOI : 1; /**< \brief IR.3 Data Overrun Interrupt */ 130 | unsigned int WUI : 1; /**< \brief IR.4 Wake-Up Interrupt */ 131 | unsigned int EPI : 1; /**< \brief IR.5 Error Passive Interrupt */ 132 | unsigned int ALI : 1; /**< \brief IR.6 Arbitration Lost Interrupt */ 133 | unsigned int BEI : 1; /**< \brief IR.7 Bus Error Interrupt */ 134 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 135 | } B; 136 | } IR; 137 | union { 138 | uint32_t U; /**< \brief Unsigned access */ 139 | struct { 140 | unsigned int RIE : 1; /**< \brief IER.0 Receive Interrupt Enable */ 141 | unsigned int TIE : 1; /**< \brief IER.1 Transmit Interrupt Enable */ 142 | unsigned int EIE : 1; /**< \brief IER.2 Error Interrupt Enable */ 143 | unsigned int DOIE : 1; /**< \brief IER.3 Data Overrun Interrupt Enable */ 144 | unsigned int WUIE : 1; /**< \brief IER.4 Wake-Up Interrupt Enable */ 145 | unsigned int EPIE : 1; /**< \brief IER.5 Error Passive Interrupt Enable */ 146 | unsigned int ALIE : 1; /**< \brief IER.6 Arbitration Lost Interrupt Enable */ 147 | unsigned int BEIE : 1; /**< \brief IER.7 Bus Error Interrupt Enable */ 148 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 149 | } B; 150 | } IER; 151 | uint32_t RESERVED0; 152 | union { 153 | uint32_t U; /**< \brief Unsigned access */ 154 | struct { 155 | unsigned int BRP : 6; /**< \brief BTR0[5:0] Baud Rate Prescaler */ 156 | unsigned int SJW : 2; /**< \brief BTR0[7:6] Synchronization Jump Width*/ 157 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 158 | } B; 159 | } BTR0; 160 | union { 161 | uint32_t U; /**< \brief Unsigned access */ 162 | struct { 163 | unsigned int TSEG1 : 4; /**< \brief BTR1[3:0] Timing Segment 1 */ 164 | unsigned int TSEG2 : 3; /**< \brief BTR1[6:4] Timing Segment 2*/ 165 | unsigned int SAM : 1; /**< \brief BTR1.7 Sampling*/ 166 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 167 | } B; 168 | } BTR1; 169 | union { 170 | uint32_t U; /**< \brief Unsigned access */ 171 | struct { 172 | unsigned int OCMODE : 2; /**< \brief OCR[1:0] Output Control Mode, see # */ 173 | unsigned int OCPOL0 : 1; /**< \brief OCR.2 Output Control Polarity 0 */ 174 | unsigned int OCTN0 : 1; /**< \brief OCR.3 Output Control Transistor N0 */ 175 | unsigned int OCTP0 : 1; /**< \brief OCR.4 Output Control Transistor P0 */ 176 | unsigned int OCPOL1 : 1; /**< \brief OCR.5 Output Control Polarity 1 */ 177 | unsigned int OCTN1 : 1; /**< \brief OCR.6 Output Control Transistor N1 */ 178 | unsigned int OCTP1 : 1; /**< \brief OCR.7 Output Control Transistor P1 */ 179 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 180 | } B; 181 | } OCR; 182 | uint32_t RESERVED1[2]; 183 | union { 184 | uint32_t U; /**< \brief Unsigned access */ 185 | struct { 186 | unsigned int ALC : 8; /**< \brief ALC[7:0] Arbitration Lost Capture */ 187 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 188 | } B; 189 | } ALC; 190 | union { 191 | uint32_t U; /**< \brief Unsigned access */ 192 | struct { 193 | unsigned int ECC : 8; /**< \brief ECC[7:0] Error Code Capture */ 194 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 195 | } B; 196 | } ECC; 197 | union { 198 | uint32_t U; /**< \brief Unsigned access */ 199 | struct { 200 | unsigned int EWLR : 8; /**< \brief EWLR[7:0] Error Warning Limit */ 201 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 202 | } B; 203 | } EWLR; 204 | union { 205 | uint32_t U; /**< \brief Unsigned access */ 206 | struct { 207 | unsigned int RXERR : 8; /**< \brief RXERR[7:0] Receive Error Counter */ 208 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 209 | } B; 210 | } RXERR; 211 | union { 212 | uint32_t U; /**< \brief Unsigned access */ 213 | struct { 214 | unsigned int TXERR : 8; /**< \brief TXERR[7:0] Transmit Error Counter */ 215 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 216 | } B; 217 | } TXERR; 218 | 219 | union { 220 | struct { 221 | uint32_t CODE[4]; /**< \brief Acceptance Message ID */ 222 | uint32_t MASK[4]; /**< \brief Acceptance Mask */ 223 | uint32_t RESERVED2[5]; 224 | } ACC; /**< \brief Acceptance filtering */ 225 | struct { 226 | CAN_FIR_t FIR; /**< \brief Frame information record */ 227 | union { 228 | struct { 229 | uint32_t ID[2]; /**< \brief Standard frame message-ID*/ 230 | uint32_t data[8]; /**< \brief Standard frame payload */ 231 | uint32_t reserved[2]; 232 | } STD; /**< \brief Standard frame format */ 233 | struct { 234 | uint32_t ID[4]; /**< \brief Extended frame message-ID*/ 235 | uint32_t data[8]; /**< \brief Extended frame payload */ 236 | } EXT; /**< \brief Extended frame format */ 237 | } TX_RX; /**< \brief RX/TX interface */ 238 | } FCTRL; /**< \brief Function control regs */ 239 | } MBX_CTRL; /**< \brief Mailbox control */ 240 | union { 241 | uint32_t U; /**< \brief Unsigned access */ 242 | struct { 243 | unsigned int RMC : 8; /**< \brief RMC[7:0] RX Message Counter */ 244 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved Enable */ 245 | } B; 246 | } RMC; 247 | union { 248 | uint32_t U; /**< \brief Unsigned access */ 249 | struct { 250 | unsigned int RBSA : 8; /**< \brief RBSA[7:0] RX Buffer Start Address */ 251 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved Enable */ 252 | } B; 253 | } RBSA; 254 | union { 255 | uint32_t U; /**< \brief Unsigned access */ 256 | struct { 257 | unsigned int COD : 3; /**< \brief CDR[2:0] CLKOUT frequency selector based of fOSC*/ 258 | unsigned int COFF : 1; /**< \brief CDR.3 CLKOUT off*/ 259 | unsigned int reserved_1 : 1; /**< \brief \internal Reserved */ 260 | unsigned int 261 | RXINTEN : 1; /**< \brief CDR.5 This bit allows the TX1 output to be used as a dedicated receive interrupt 262 | output*/ 263 | unsigned int 264 | CBP : 1; /**< \brief CDR.6 allows to bypass the CAN input comparator and is only possible in reset mode.*/ 265 | unsigned int 266 | CAN_M : 1; /**< \brief CDR.7 If CDR.7 is at logic 0 the CAN controller operates in BasicCAN mode. If set to 267 | logic 1 the CAN controller operates in PeliCAN mode. Write access is only possible in reset 268 | mode*/ 269 | unsigned int reserved_24 : 24; /**< \brief \internal Reserved */ 270 | } B; 271 | } CDR; 272 | uint32_t IRAM[2]; 273 | } CAN_Module_t; 274 | 275 | #ifdef __cplusplus 276 | } 277 | #endif 278 | 279 | #endif /* __DRIVERS_CAN_REGDEF_H_ */ 280 | -------------------------------------------------------------------------------- /Raybot_test/lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /Raybot_test/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:esp32dev] 12 | platform = espressif32 13 | board = esp32dev 14 | framework = arduino 15 | upload_port = COM6 16 | monitor_port = COM6 17 | monitor_speed = 115200 18 | board_build.partitions = huge_app.csv 19 | lib_deps = 20 | aasim-a/AsyncTimer@^2.4.0 21 | esphome/ESPAsyncWebServer-esphome@^3.1.0 22 | bblanchon/ArduinoJson@^6.21.4 23 | esphome/AsyncTCP-esphome@^2.0.1 24 | s00500/ESPUI@^2.2.3 25 | -------------------------------------------------------------------------------- /Raybot_test/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "test.h" 2 | #include 3 | #include 4 | 5 | const byte DNS_PORT = 53; 6 | IPAddress apIP(192, 168, 4, 1); 7 | DNSServer dnsServer; 8 | 9 | #if defined(ESP32) 10 | #include 11 | #else 12 | // esp8266 13 | 14 | #ifndef MMU_IRAM_HEAP 15 | #warning Try MMU option '2nd heap shared' in 'tools' IDE menu (cf. https://arduino-esp8266.readthedocs.io/en/latest/mmu.html#option-summary) 16 | #warning use decorators: { HeapSelectIram doAllocationsInIRAM; ESPUI.addControl(...) ... } (cf. https://arduino-esp8266.readthedocs.io/en/latest/mmu.html#how-to-select-heap) 17 | #warning then check http:///heap 18 | #endif // MMU_IRAM_HEAP 19 | #ifndef DEBUG_ESP_OOM 20 | #error on ESP8266 and ESPUI, you must define OOM debug option when developping 21 | #endif 22 | #endif 23 | 24 | const char *ssid = "2024"; 25 | const char *password = "12345678"; 26 | const char *hostname = "espressif"; 27 | 28 | uint16_t status; 29 | uint16_t button1; 30 | uint16_t button2; 31 | uint16_t button3; 32 | uint16_t button4; 33 | uint16_t button5; 34 | uint16_t button6; 35 | uint16_t button7; 36 | uint16_t button8; 37 | uint16_t millisLabelId; 38 | uint16_t switchOne; 39 | 40 | void numberCall_1(Control *sender_1, int type_1) 41 | { 42 | Serial.println(sender_1->value); 43 | Speed = sender_1->value.toInt(); 44 | } 45 | void numberCall_2(Control *sender_2, int type_2) 46 | { 47 | Serial.println(sender_2->value); 48 | Increment = sender_2->value.toInt(); 49 | } 50 | void numberCall_3(Control *sender_3, int type_3) 51 | { 52 | Serial.println(sender_3->value); 53 | Increment = sender_3->value.toInt(); 54 | } 55 | 56 | void buttonCallback_1(Control *sender, int type) 57 | { 58 | switch (type) 59 | { 60 | case B_DOWN: 61 | Serial.println("Button_1 DOWN"); 62 | break; 63 | 64 | case B_UP: 65 | Serial.println("Button_1 UP"); 66 | panduan = 1; 67 | break; 68 | } 69 | } 70 | 71 | void buttonCallback_2(Control *sender, int type) 72 | { 73 | switch (type) 74 | { 75 | case B_DOWN: 76 | Serial.println("Button_2 DOWN"); 77 | break; 78 | 79 | case B_UP: 80 | Serial.println("Button_2 UP"); 81 | panduan = 2; 82 | break; 83 | } 84 | } 85 | 86 | void buttonCallback_3(Control *sender, int type) 87 | { 88 | switch (type) 89 | { 90 | case B_DOWN: 91 | Serial.println("Button_3 DOWN"); 92 | break; 93 | 94 | case B_UP: 95 | Serial.println("Button_3 UP"); 96 | panduan = 3; 97 | break; 98 | } 99 | } 100 | void buttonCallback_4(Control *sender, int type) 101 | { 102 | switch (type) 103 | { 104 | case B_DOWN: 105 | Serial.println("Button_4 DOWN"); 106 | break; 107 | 108 | case B_UP: 109 | Serial.println("Button_4 UP"); 110 | panduan = 4; 111 | break; 112 | } 113 | } 114 | void buttonCallback_5(Control *sender, int type) 115 | { 116 | switch (type) 117 | { 118 | case B_DOWN: 119 | Serial.println("Button_5 DOWN"); 120 | break; 121 | 122 | case B_UP: 123 | Serial.println("Button_5 UP"); 124 | panduan = 5; 125 | break; 126 | } 127 | } 128 | void buttonCallback_6(Control *sender, int type) 129 | { 130 | switch (type) 131 | { 132 | case B_DOWN: 133 | Serial.println("Button_6 DOWN"); 134 | break; 135 | 136 | case B_UP: 137 | Serial.println("Button_6 UP"); 138 | panduan = 6; 139 | break; 140 | } 141 | } 142 | void buttonCallback_7(Control *sender, int type) 143 | { 144 | switch (type) 145 | { 146 | case B_DOWN: 147 | Serial.println("Button_5 DOWN"); 148 | break; 149 | 150 | case B_UP: 151 | Serial.println("Button_5 UP"); 152 | panduan = 7; 153 | break; 154 | } 155 | }void buttonCallback_8(Control *sender, int type) 156 | { 157 | switch (type) 158 | { 159 | case B_DOWN: 160 | Serial.println("Button_5 DOWN"); 161 | break; 162 | 163 | case B_UP: 164 | Serial.println("Button_5 UP"); 165 | panduan = 8; 166 | break; 167 | } 168 | } 169 | 170 | CAN_device_t CAN_cfg; // CAN Config (CAN配置) 171 | unsigned long previousMillis = 0; // will store last time a CAN Message was send (将存储最后一次发送的CAN消息) 172 | const int interval = 10; // interval at which send CAN Messages (milliseconds) (发送CAN消息的时间间隔(毫秒)) 173 | const int rx_queue_size = 10; // Receive Queue size (接收队列大小) 174 | 175 | void setup() 176 | { 177 | pinMode(PIN_5V_EN, OUTPUT); 178 | digitalWrite(PIN_5V_EN, HIGH); 179 | 180 | pinMode(CAN_SE_PIN, OUTPUT); 181 | digitalWrite(CAN_SE_PIN, LOW); 182 | 183 | // 初始化IO32，开启引脚的输出模式 184 | pinMode(32, OUTPUT); 185 | digitalWrite(32, HIGH); 186 | 187 | ESPUI.setVerbosity(Verbosity::VerboseJSON); 188 | Serial.begin(115200); 189 | CAN_cfg.speed = CAN_SPEED_1000KBPS; 190 | CAN_cfg.tx_pin_id = GPIO_NUM_27; 191 | CAN_cfg.rx_pin_id = GPIO_NUM_26; 192 | CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t)); 193 | // Init CAN Module (初始化CAN模块) 194 | ESP32Can.CANInit(); 195 | 196 | #if defined(ESP32) 197 | WiFi.setHostname(hostname); 198 | #else 199 | WiFi.hostname(hostname); 200 | #endif 201 | 202 | // try to connect to existing network 203 | WiFi.begin(ssid, password); 204 | Serial.print("\n\nTry to connect to existing network"); 205 | 206 | { 207 | uint8_t timeout = 10; 208 | 209 | // Wait for connection, 5s timeout 210 | do 211 | { 212 | delay(500); 213 | Serial.print("."); 214 | timeout--; 215 | } while (timeout && WiFi.status() != WL_CONNECTED); 216 | 217 | // not connected -> create hotspot 218 | if (WiFi.status() != WL_CONNECTED) 219 | { 220 | Serial.print("\n\nCreating hotspot"); 221 | 222 | WiFi.mode(WIFI_AP); 223 | delay(100); 224 | WiFi.softAPConfig(apIP, apIP, IPAddress(255, 255, 255, 0)); 225 | #if defined(ESP32) 226 | uint32_t chipid = 0; 227 | for (int i = 0; i < 17; i = i + 8) 228 | { 229 | chipid |= ((ESP.getEfuseMac() >> (40 - i)) & 0xff) << i; 230 | } 231 | #else 232 | uint32_t chipid = ESP.getChipId(); 233 | #endif 234 | char ap_ssid[25]; 235 | snprintf(ap_ssid, 26, "ESPUI-%08X", chipid); 236 | WiFi.softAP(ap_ssid); 237 | 238 | timeout = 5; 239 | 240 | do 241 | { 242 | delay(500); 243 | Serial.print("."); 244 | timeout--; 245 | } while (timeout); 246 | } 247 | } 248 | 249 | dnsServer.start(DNS_PORT, "*", apIP); 250 | 251 | Serial.println("\n\nWiFi parameters:"); 252 | Serial.print("Mode: "); 253 | Serial.println(WiFi.getMode() == WIFI_AP ? "Station" : "Client"); 254 | Serial.print("IP address: "); 255 | Serial.println(WiFi.getMode() == WIFI_AP ? WiFi.softAPIP() : WiFi.localIP()); 256 | #ifdef ESP8266 257 | { 258 | HeapSelectIram doAllocationsInIRAM; 259 | #endif 260 | 261 | ESPUI.addControl(ControlType::Number, "速度:", "0", ControlColor::Sunflower, Control::noParent, &numberCall_1); 262 | ESPUI.addControl(ControlType::Number, "增量:", "0", ControlColor::Dark, Control::noParent, &numberCall_2); 263 | button1 = ESPUI.addControl( 264 | ControlType::Button, " ", "正180", ControlColor::Peterriver, Control::noParent, &buttonCallback_1); 265 | button2 = ESPUI.addControl( 266 | ControlType::Button, " ", "负180", ControlColor::Peterriver, Control::noParent, &buttonCallback_2); 267 | button3 = ESPUI.addControl( 268 | ControlType::Button, " ", "归0", ControlColor::Peterriver, Control::noParent, &buttonCallback_3); 269 | button4 = ESPUI.addControl( 270 | ControlType::Button, " ", "低30", ControlColor::Peterriver, Control::noParent, &buttonCallback_4); 271 | button5 = ESPUI.addControl( 272 | ControlType::Button, " ", "抬30", ControlColor::Peterriver, Control::noParent, &buttonCallback_5); 273 | button6 = ESPUI.addControl( 274 | ControlType::Button, " ", "左30", ControlColor::Peterriver, Control::noParent, &buttonCallback_6); 275 | button7 = ESPUI.addControl( 276 | ControlType::Button, " ", "右30", ControlColor::Peterriver, Control::noParent, &buttonCallback_7); 277 | button8 = ESPUI.addControl( 278 | ControlType::Button, " ", "归零", ControlColor::Peterriver, Control::noParent, &buttonCallback_8); 279 | 280 | ESPUI.begin("Demo", "demo", "123456"); 281 | 282 | #ifdef ESP8266 283 | } // HeapSelectIram 284 | #endif 285 | 286 | // put your setup code here, to run once: (将您的设置代码放在此处，运行一次：) 287 | 288 | num1 = 0.01 * read_rd(1, 0x92); // 读取1轴位置 289 | delay(100); 290 | run(1, 200, num1); 291 | delay(100); 292 | num2 = 0.01 * read_rd(2, 0x92); // 读取2轴位置 293 | delay(100); 294 | run(2, 200, num2); 295 | delay(100); 296 | num3 = 0.01 * read_rd(3, 0x92); // 读取3轴位置 297 | delay(100); 298 | run(3, 200, num3); 299 | delay(1500); 300 | panduan = -1; 301 | } 302 | 303 | void loop() 304 | { 305 | // put your setup code here, to loop run: (将您的设置代码放在此处，循环运行:) 306 | dnsServer.processNextRequest(); 307 | switch (panduan) 308 | { 309 | case 1: 310 | run(1, 500, num1 + 1080); // 正转到180 311 | delay(100); 312 | panduan = -1; 313 | break; 314 | case 2: 315 | run(1, 500, num1 - 1080); // 反转到180 316 | delay(100); 317 | panduan = -1; 318 | break; 319 | case 3: 320 | run(1, 500, num1); // 转到0 321 | delay(100); 322 | panduan = -1; 323 | break; 324 | 325 | case 4: 326 | run(2, 500, num2 + 180); // 低30 327 | delay(100); 328 | panduan = -1; 329 | break; 330 | 331 | case 5: 332 | run(2, 500, num2); // 转到0 333 | delay(100); 334 | panduan = -1; 335 | break; 336 | 337 | case 6: 338 | run(3, 500, num3 +180); // 转到0 339 | delay(100); 340 | panduan = -1; 341 | break; 342 | 343 | case 7: 344 | 345 | run(3, 500, num3- 180); // 转到0 346 | delay(100); 347 | panduan = -1; 348 | break; 349 | 350 | case 8: 351 | run(3, 500, num3); // 转到0 352 | delay(100); 353 | panduan = -1; 354 | break; 355 | } 356 | } 357 | -------------------------------------------------------------------------------- /Raybot_test/src/test.cpp: -------------------------------------------------------------------------------- 1 | #include "test.h" 2 | 3 | int txid; 4 | int num1; 5 | int num2; 6 | int num3; 7 | int Speed; 8 | int Increment; 9 | int panduan; 10 | 11 | /*运行*/ 12 | 13 | void run(int id, int speed, int position) 14 | { 15 | // CAN_frame_t rx_frame; 16 | CAN_frame_t tx_frame; 17 | tx_frame.FIR.B.FF = CAN_frame_std; 18 | tx_frame.FIR.B.RTR =CAN_no_RTR; 19 | tx_frame.MsgID = 0x140 + id; 20 | tx_frame.FIR.B.DLC = 8; 21 | tx_frame.data.u8[0] = 0xA4; 22 | tx_frame.data.u8[1] = 0x00; 23 | int32_t *p1 = (int32_t *)&tx_frame.data.u8[2]; 24 | int32_t *p2 = (int32_t *)&tx_frame.data.u8[4]; 25 | *p1 = (int32_t)speed; 26 | *p2 = (int32_t)100 * position; 27 | ESP32Can.CANWriteFrame(&tx_frame); 28 | } 29 | 30 | /*发送指令读取电机运行时的数据 31 | */ 32 | int read_rd(int id, int suoyin) 33 | { 34 | 35 | CAN_frame_t tx_frame; 36 | int rxdata; 37 | tx_frame.FIR.B.FF = CAN_frame_std; 38 | tx_frame.FIR.B.RTR =CAN_no_RTR; 39 | tx_frame.MsgID = 0x140 + id; 40 | tx_frame.FIR.B.DLC = 8; 41 | tx_frame.data.u8[0] = suoyin; 42 | tx_frame.data.u8[1] = 0x00; 43 | tx_frame.data.u8[2] = 0x00; 44 | tx_frame.data.u8[3] = 0x00; 45 | tx_frame.data.u8[4] = 0x00; 46 | tx_frame.data.u8[5] = 0x00; 47 | tx_frame.data.u8[6] = 0x00; 48 | tx_frame.data.u8[7] = 0x00; 49 | ESP32Can.CANWriteFrame(&tx_frame); 50 | unsigned long currentMillis = millis(); 51 | CAN_frame_t rx_frame; 52 | while (1) 53 | { 54 | xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS); 55 | if (tx_frame.MsgID == rx_frame.MsgID) 56 | { 57 | rxdata = *(int *)&rx_frame.data.u8[1]; 58 | break; 59 | } 60 | } 61 | return rxdata; 62 | } 63 | -------------------------------------------------------------------------------- /Raybot_test/src/test.h: -------------------------------------------------------------------------------- 1 | #ifndef __TEST_H__ 2 | #define __TEST_H__ 3 | #include 4 | #include "config.h" //引脚配置 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | extern int txid; 14 | extern int num1; 15 | extern int num2; 16 | extern int num3; 17 | extern int Speed; 18 | extern int Increment; 19 | extern int panduan; 20 | 21 | void run(int, int, int); // 运行 22 | int read_rd(int, int); // 读取位置 23 | 24 | #endif -------------------------------------------------------------------------------- /URDF/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(URDF) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package() 8 | 9 | find_package(roslaunch) 10 | 11 | foreach(dir config launch meshes urdf) 12 | install(DIRECTORY ${dir}/ 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 14 | endforeach(dir) 15 | -------------------------------------------------------------------------------- /URDF/config/joint_names_URDF.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['', 'ia1', 'ir1', 'ir2', 'ir7', 'ir8', 'il1', 'il2', 'il7', 'il8', ] 2 | -------------------------------------------------------------------------------- /URDF/export.log: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/export.log -------------------------------------------------------------------------------- /URDF/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 11 | 15 | 20 | -------------------------------------------------------------------------------- /URDF/launch/display_humanoid.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 11 | 15 | 20 | -------------------------------------------------------------------------------- /URDF/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 9 | 15 | 20 | -------------------------------------------------------------------------------- /URDF/meshes/ai.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ai.STL -------------------------------------------------------------------------------- /URDF/meshes/aia1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/aia1.STL -------------------------------------------------------------------------------- /URDF/meshes/aia2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/aia2.STL -------------------------------------------------------------------------------- /URDF/meshes/ail1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ail1.STL -------------------------------------------------------------------------------- /URDF/meshes/ail2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ail2.STL -------------------------------------------------------------------------------- /URDF/meshes/ail3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ail3.STL -------------------------------------------------------------------------------- /URDF/meshes/ail4.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ail4.STL -------------------------------------------------------------------------------- /URDF/meshes/ail5.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ail5.STL -------------------------------------------------------------------------------- /URDF/meshes/ail6.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ail6.STL -------------------------------------------------------------------------------- /URDF/meshes/ail7.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ail7.STL -------------------------------------------------------------------------------- /URDF/meshes/ail8.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ail8.STL -------------------------------------------------------------------------------- /URDF/meshes/air1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/air1.STL -------------------------------------------------------------------------------- /URDF/meshes/air2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/air2.STL -------------------------------------------------------------------------------- /URDF/meshes/air3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/air3.STL -------------------------------------------------------------------------------- /URDF/meshes/air4.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/air4.STL -------------------------------------------------------------------------------- /URDF/meshes/air5.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/air5.STL -------------------------------------------------------------------------------- /URDF/meshes/air6.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/air6.STL -------------------------------------------------------------------------------- /URDF/meshes/air7.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/air7.STL -------------------------------------------------------------------------------- /URDF/meshes/air8.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/air8.STL -------------------------------------------------------------------------------- /URDF/meshes/ait1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ait1.STL -------------------------------------------------------------------------------- /URDF/meshes/ait2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ait2.STL -------------------------------------------------------------------------------- /URDF/meshes/ait3.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RaybotEvolution/Raybot/c20d73763813b5e0da40f2883cfc4467726c50ae/URDF/meshes/ait3.STL -------------------------------------------------------------------------------- /URDF/package.xml: -------------------------------------------------------------------------------- 1 | 2 | URDF 3 | 1.0.0 4 | 5 |

URDF Description package for URDF

6 |

This package contains configuration data, 3D models and launch files 7 | for URDF robot

8 | 9 | TODO 10 | 11 | BSD 12 | catkin 13 | roslaunch 14 | robot_state_publisher 15 | rviz 16 | joint_state_publisher_gui 17 | gazebo 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /URDF/rviz/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 407 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | ai: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | aia1: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | aia2: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | ail1: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | ail2: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | ail3: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | ail4: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | ail5: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | ail6: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | ail7: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | Value: true 114 | ail8: 115 | Alpha: 1 116 | Show Axes: false 117 | Show Trail: false 118 | Value: true 119 | air1: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | Value: true 124 | air2: 125 | Alpha: 1 126 | Show Axes: false 127 | Show Trail: false 128 | Value: true 129 | air3: 130 | Alpha: 1 131 | Show Axes: false 132 | Show Trail: false 133 | Value: true 134 | air4: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | Value: true 139 | air5: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Value: true 144 | air6: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | Value: true 149 | air7: 150 | Alpha: 1 151 | Show Axes: false 152 | Show Trail: false 153 | Value: true 154 | air8: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | Value: true 159 | ait1: 160 | Alpha: 1 161 | Show Axes: false 162 | Show Trail: false 163 | Value: true 164 | ait2: 165 | Alpha: 1 166 | Show Axes: false 167 | Show Trail: false 168 | Value: true 169 | ait3: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | Value: true 174 | Name: RobotModel 175 | Robot Description: robot_description 176 | TF Prefix: "" 177 | Update Interval: 0 178 | Value: true 179 | Visual Enabled: true 180 | Enabled: true 181 | Global Options: 182 | Background Color: 48; 48; 48 183 | Default Light: true 184 | Fixed Frame: ai 185 | Frame Rate: 30 186 | Name: root 187 | Tools: 188 | - Class: rviz/Interact 189 | Hide Inactive Objects: true 190 | - Class: rviz/MoveCamera 191 | - Class: rviz/Select 192 | - Class: rviz/FocusCamera 193 | - Class: rviz/Measure 194 | - Class: rviz/SetInitialPose 195 | Theta std deviation: 0.2617993950843811 196 | Topic: /initialpose 197 | X std deviation: 0.5 198 | Y std deviation: 0.5 199 | - Class: rviz/SetGoal 200 | Topic: /move_base_simple/goal 201 | - Class: rviz/PublishPoint 202 | Single click: true 203 | Topic: /clicked_point 204 | Value: true 205 | Views: 206 | Current: 207 | Class: rviz/Orbit 208 | Distance: 6.4342041015625 209 | Enable Stereo Rendering: 210 | Stereo Eye Separation: 0.05999999865889549 211 | Stereo Focal Distance: 1 212 | Swap Stereo Eyes: false 213 | Value: false 214 | Field of View: 0.7853981852531433 215 | Focal Point: 216 | X: 0 217 | Y: 0 218 | Z: 0 219 | Focal Shape Fixed Size: true 220 | Focal Shape Size: 0.05000000074505806 221 | Invert Z Axis: false 222 | Name: Current View 223 | Near Clip Distance: 0.009999999776482582 224 | Pitch: 0.785398006439209 225 | Target Frame: 226 | Yaw: 0.785398006439209 227 | Saved: ~ 228 | Window Geometry: 229 | Displays: 230 | collapsed: false 231 | Height: 704 232 | Hide Left Dock: false 233 | Hide Right Dock: false 234 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000222fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000222000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000222fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000222000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006501000000000000050e000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000029d0000022200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 235 | Selection: 236 | collapsed: false 237 | Time: 238 | collapsed: false 239 | Tool Properties: 240 | collapsed: false 241 | Views: 242 | collapsed: false 243 | Width: 1294 244 | X: 72 245 | Y: 27 246 | -------------------------------------------------------------------------------- /URDF/urdf/URDF.csv: -------------------------------------------------------------------------------- 1 | Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity 2 | ai,0.0096555,-0.14212,-0.038542,0,0,0,5.5457,0.028923,-0.00050649,7.6006E-06,0.046711,0.00014985,0.022605,0,0,0,0,0,0,package://URDF/meshes/ai.STL,1,1,1,1,0,0,0,0,0,0,package://URDF/meshes/ai.STL,,a1-1,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, 3 | aia1,0.0122165500894778,0.0631182438776054,-0.00692252450444186,0,0,0,3.50978575560884,0.0107160246499877,-0.000359621582143605,4.07016624714828E-05,0.0124005795141034,-1.524781901073E-06,0.012386384323181,0,0,0,0,0,0,package://URDF/meshes/aia1.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/aia1.STL,,a2-1,Origin_ia1,Axis_ia1,ia1,continuous,-0.0079361,0.014041,-0.00022442,1.6815,-0.028271,0,ai,0,0.99387,-0.11052,0,0,0,0,,,,,,,, 4 | aia2,0.00275148999525586,0.0904963085395856,0.0206857738259683,0,0,0,11.5092978911004,0.0488817529527658,0.000298183073814911,3.87850448289944E-07,0.0789799485369125,-0.00122753004573844,0.111894811595064,0,0,0,0,0,0,package://URDF/meshes/aia2.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/aia2.STL,,a3-1,Origin_ia2,Axis_ia2,ia2,fixed,2.93308576122722E-05,0.188056300534155,-0.0208868045568571,-0.110744460506445,0,0,aia1,0,0,1,0,0,0,0,,,,,,,, 5 | air1,-0.0867882598030639,0.0501262855873437,-0.00496010592416558,0,0,0,0.704319744476176,0.00102192742218782,5.18316851700758E-05,-0.000164786809473083,0.00108196027992708,9.54818424027483E-05,0.00117996029784948,0,0,0,0,0,0,package://URDF/meshes/air1.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/air1.STL,,r1-1,Origin_ir1,Axis_ir1,ir1,continuous,0.0971134252482515,0.153771425983859,0,-3.14159265358979,0,3.14159265358978,aia2,0.866025403784438,-0.5,0,0,0,0,0,,,,,,,, 6 | air2,-0.0221091585052234,-0.00947394911243532,-0.0713325927305242,0,0,0,2.16696749170869,0.00235923373053802,-0.000466751355116642,0.00042805486522134,0.00351160698856537,0.000286782243418935,0.00405898161391467,0,0,0,0,0,0,package://URDF/meshes/air2.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/air2.STL,,r2-1,Origin_ir2,Axis_ir2,ir2,continuous,-0.103808135150164,0.0599336547730216,-0.00199999999999997,0,0,0.523598775598286,air1,0,0,1,0,0,0,0,,,,,,,, 7 | air3,-0.310723078744321,0.133593457504276,-0.0460271418084565,0,0,0,2.43210030597648,0.00318621353488848,-0.000704673937050692,-1.48997132472017E-06,0.00497330748293435,-4.62910674210498E-08,0.00480738048481119,0,0,0,0,0,0,package://URDF/meshes/air3.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/air3.STL,,r3-1,Origin_ir3,Axis_ir3,ir3,fixed,-0.115052758244909,-0.0312772228311444,0.274355715851498,-1.5707963267949,-1.0471975511966,1.0471975511966,air2,-0.866025403784439,0.5,0,0,0,0,0,,,,,,,, 8 | air4,-0.306811586113211,0.0543922506429295,-0.0459893323925083,0,0,0,1.60575264215698,0.00171417926629064,-0.000427404427054719,2.88573008654781E-07,0.00195481134290395,1.86832341871254E-07,0.00186489394892792,0,0,0,0,0,0,package://URDF/meshes/air4.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/air4.STL,,r4-1,Origin_ir4,Axis_ir4,ir4,fixed,0,0,0,0,0,0,air3,0.866025403784439,-0.5,0,0,0,0,0,,,,,,,, 9 | air5,-0.400491002863523,-0.0149195507388673,-0.0460243732362791,0,0,0,2.43210030636316,0.00318561159001296,-0.000704701925284907,-1.68124491102077E-06,0.00497354044938666,3.72017684928891E-08,0.00480774946362265,0,0,0,0,0,0,package://URDF/meshes/air5.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/air5.STL,,r5-1,Origin_ir5,Axis_ir5,ir5,fixed,0,0,0,0,0,0,air4,-0.866025403784439,0.5,0,0,0,0,0,,,,,,,, 10 | air6,-0.396341248975106,-0.0942717174606415,-0.0460104212942954,0,0,0,1.60575264215694,0.00171398866333832,-0.000427509349226874,5.8493043760927E-09,0.00195501969069459,3.26534102097182E-07,0.00186487620408945,0,0,0,0,0,0,package://URDF/meshes/air6.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/air6.STL,,r6-1,Origin_ir6,Axis_ir6,ir6,fixed,0,0,0,0,0,0,air5,0.866025403784439,-0.5,0,0,0,0,0,,,,,,,, 11 | air7,-0.0728097455352885,-0.1261240051641,1.70512777112886E-05,0,0,0,2.39752294354751,0.0012687393901073,0.000168151144527475,-3.04642830826539E-07,0.001463313975307,2.54712948479235E-07,0.00117306166024798,0,0,0,0,0,0,package://URDF/meshes/air7.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/air7.STL,,r7-1,Origin_ir7,Axis_ir7,ir7,continuous,-0.42328075115025,-0.0824323351551474,-0.0460000000000423,-1.64545323690127,-1.04598885548756,1.1118372926814,air6,0.499999999999995,0.866025403784442,0,0,0,0,0,,,,,,,, 12 | air8,-0.0256124603916125,-0.0439679593331598,-9.86050862532076E-05,0,0,0,0.39028900516816,0.00011213379731787,3.06765443482523E-05,2.44292075121941E-07,0.000147872106153398,-2.20108758996571E-07,9.42772763134408E-05,0,0,0,0,0,0,package://URDF/meshes/air8.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/air8.STL,,r8-1,Origin_ir8,Axis_ir8,ir8,continuous,-0.0495231049974948,-0.085776534004209,0,0.0186968020728057,0.0323706130972615,0.000302648714584661,air7,0.499999999999999,0.866025403784439,0,0,0,0,0,,,,,,,, 13 | ail1,0.0314996586787025,0.018110986495873,-0.00495965585467567,0,0,0,0.704317483213798,0.00102054589410166,-5.10348006602906E-05,0.000164763374924484,0.00108334133408607,9.55071581716024E-05,0.00117995821420813,0,0,0,0,0,0,package://URDF/meshes/ail1.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ail1.STL,,l1-1,Origin_il1,Axis_il1,il1,continuous,-0.152442926705883,0.185715928544454,0,-3.13362768307923,-0.00459849683182837,3.14157434001486,aia2,0.866025403784445,0.499999999999989,0,0,0,0,0,,,,,,,, 14 | ail2,0.0254230133562362,-0.00487000210777616,-0.0731799326032108,0,0,0,2.16696451297082,0.00236378749188235,0.000462102165751906,-0.000439308791472968,0.00351556883965291,0.000293029666338872,0.00405037989081645,0,0,0,0,0,0,package://URDF/meshes/ail2.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ail2.STL,,l2-1,Origin_il2,Axis_il2,il2,continuous,0.0433012701892223,0.0249999999999994,0,0.00336630175696954,-0.00336635386910309,-0.52358329515933,ail1,0.00459848062507821,-0.00796480208025818,-0.999957707057535,0,0,0,0,,,,,,,, 15 | ail3,0.282115881288905,0.123680479739269,-0.0449326076320535,0,0,0,1.74913628400847,0.00291267479222441,-0.000273526909035704,2.77544209116255E-06,0.00192837816529404,4.02456722990272E-06,0.00267897419074648,0,0,0,0,0,0,package://URDF/meshes/ail3.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ail3.STL,,l3-1,Origin_il3,Axis_il3,il3,fixed,0.0407134685302632,-0.0746066995021419,-0.363589645435843,1.5706244129294,-1.0563942468247,-1.03785118325407,ail2,-0.866025403784446,-0.499999999999988,0,0,0,0,0,,,,,,,, 16 | ail4,0.351023016424299,0.0796276581927194,-0.0419538130734384,0,0,0,1.6057526420519,0.00226389095320379,0.000108089324605775,1.47115061675745E-05,0.00141181624420584,6.39817759898937E-05,0.0018581773606691,0,0,0,0,0,0,package://URDF/meshes/ail4.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ail4.STL,,l4-1,Origin_il4,Axis_il4,il4,fixed,0.00332392501798523,-0.00575720701169549,0.0114072634871511,0.114900076489375,0.066095305063605,0.00380274076774772,ail3,0.866025403784445,0.499999999999989,0,0,0,0,0,,,,,,,, 17 | ail5,0.365243125976542,-0.0273001624763137,-0.0277409109306254,0,0,0,1.74913628404884,0.00290920090084644,-0.000273029467014606,4.96572447315667E-05,0.00193706948379787,7.14721881758528E-05,0.00267375676364664,0,0,0,0,0,0,package://URDF/meshes/ail5.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ail5.STL,,l5-1,Origin_il5,Axis_il5,il5,fixed,0,0,0,0,0,0,ail4,-0.866025403784445,-0.499999999999989,0,0,0,0,0,,,,,,,, 18 | ail6,0.434125405565277,-0.0707158587875069,-0.0174324289754678,0,0,0,1.60575264205192,0.00226373417300943,0.00010817350650982,1.50126202145728E-05,0.00141199497011959,6.41374228031122E-05,0.00185815541494991,0,0,0,0,0,0,package://URDF/meshes/ail6.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ail6.STL,,l6-1,Origin_il6,Axis_il6,il6,fixed,0,0,0,0,0,0,ail5,0.866025403784445,0.499999999999989,0,0,0,0,0,,,,,,,, 19 | ail7,0.0739381474324043,-0.128044988477842,0.021106946539547,0,0,0,2.39752294323106,0.00126717765655056,-0.000164993865319654,2.68480277280964E-05,0.0014572099241612,-4.69354826547216E-05,0.00118072744415879,0,0,0,0,0,0,package://URDF/meshes/ail7.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ail7.STL,,l7-1,Origin_il7,Axis_il7,il7,continuous,0.408546942555018,-0.0849126300792067,-0.0175147453415777,1.61198579544075,-1.1922390817189,-0.940140876488344,ail6,0.494990176923062,-0.857348135678236,0.141205166331579,0,0,0,0,,,,,,,, 20 | ail8,0.0269437270629207,-0.0470874606369525,0.00766989354287182,0,0,0,0.390288998297731,0.000112053790394088,-3.01367475415813E-05,5.25911607778448E-06,0.000146608603022263,-8.52751984598756E-06,9.56207847430063E-05,0,0,0,0,0,0,package://URDF/meshes/ail8.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ail8.STL,,l8-1,Origin_il8,Axis_il8,il8,continuous,0.048937841024224,-0.0847628270666956,0.0139604305376125,0,0,0,ail7,0.494990176923058,-0.85734813567823,0.141205166331628,0,0,0,0,,,,,,,, 21 | ait1,-0.000307397427263539,0.324641073766468,-0.00484151936474227,0,0,0,0.611983625806655,0.000142242831213401,-2.10570916942322E-06,-1.77538627897098E-05,0.000131726125327013,6.57970153029082E-06,0.000231967152519603,0,0,0,0,0,0,package://URDF/meshes/ait1.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ait1.STL,,t1-1,Origin_it1,Axis_it1,it1,fixed,0.00112486283996327,-0.0480860764694512,-0.000106386203830956,0,0.188593113474649,0,aia2,-0.187477139999061,0,0.982268966209242,0,0,0,0,,,,,,,, 22 | ait2,-0.000943686341580841,0.378308198594552,-0.000929018528262373,0,0,0,0.671346877284216,0.000303697331372261,-8.54102017564623E-06,6.97816072501396E-06,0.000152665146008923,-1.15227587359721E-05,0.000207639618916554,0,0,0,0,0,0,package://URDF/meshes/ait2.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ait2.STL,,t2-1,Origin_it2,Axis_it2,it2,fixed,0,0,0,0,0,0,ait1,0.18747713999906,0,-0.982268966209238,0,0,0,0,,,,,,,, 23 | ait3,-0.00644149463597838,0.38223344356688,0.0441309869046198,0,0,0,0.574503209026248,8.53228275097162E-05,-1.25149812817838E-08,-3.64119412309265E-05,0.000337670531632764,6.55710766381567E-08,0.000312677588147166,0,0,0,0,0,0,package://URDF/meshes/ait3.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://URDF/meshes/ait3.STL,,t3-1,Origin_it3,Axis_it3,it3,fixed,0,0,0,0,0,0,ait2,0.187477139999061,0,-0.982268966209242,0,0,0,0,,,,,,,, 24 | -------------------------------------------------------------------------------- /URDF/urdf/URDF.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 7 | 9 | 10 | 13 | 15 | 22 | 23 | 24 | 27 | 28 | 30 | 31 | 33 | 35 | 36 | 37 | 38 | 41 | 42 | 44 | 45 | 46 | 47 | 49 | 50 | 53 | 55 | 62 | 63 | 64 | 67 | 68 | 70 | 71 | 73 | 75 | 76 | 77 | 78 | 81 | 82 | 84 | 85 | 86 | 87 | 90 | 93 | 95 | 97 | 99 | 104 | 105 | 107 | 108 | 111 | 113 | 120 | 121 | 122 | 125 | 126 | 128 | 129 | 131 | 133 | 134 | 135 | 136 | 139 | 140 | 142 | 143 | 144 | 145 | 148 | 151 | 153 | 155 | 157 | 162 | 163 | 165 | 166 | 169 | 171 | 178 | 179 | 180 | 183 | 184 | 186 | 187 | 189 | 191 | 192 | 193 | 194 | 197 | 198 | 200 | 201 | 202 | 203 | 206 | 209 | 211 | 213 | 215 | 220 | 221 | 223 | 224 | 227 | 229 | 236 | 237 | 238 | 241 | 242 | 244 | 245 | 247 | 249 | 250 | 251 | 252 | 255 | 256 | 258 | 259 | 260 | 261 | 264 | 267 | 269 | 271 | 273 | 278 | 279 | 281 | 282 | 285 | 287 | 294 | 295 | 296 | 299 | 300 | 302 | 303 | 305 | 307 | 308 | 309 | 310 | 313 | 314 | 316 | 317 | 318 | 319 | 322 | 325 | 327 | 329 | 331 | 336 | 337 | 339 | 340 | 343 | 345 | 352 | 353 | 354 | 357 | 358 | 360 | 361 | 363 | 365 | 366 | 367 | 368 | 371 | 372 | 374 | 375 | 376 | 377 | 380 | 383 | 385 | 387 | 389 | 394 | 395 | 397 | 398 | 401 | 403 | 410 | 411 | 412 | 415 | 416 | 418 | 419 | 421 | 423 | 424 | 425 | 426 | 429 | 430 | 432 | 433 | 434 | 435 | 438 | 441 | 443 | 445 | 447 | 452 | 453 | 455 | 456 | 459 | 461 | 468 | 469 | 470 | 473 | 474 | 476 | 477 | 479 | 481 | 482 | 483 | 484 | 487 | 488 | 490 | 491 | 492 | 493 | 496 | 499 | 501 | 503 | 505 | 510 | 511 | 513 | 514 | 517 | 519 | 526 | 527 | 528 | 531 | 532 | 534 | 535 | 537 | 539 | 540 | 541 | 542 | 545 | 546 | 548 | 549 | 550 | 551 | 554 | 557 | 559 | 561 | 563 | 568 | 569 | 571 | 572 | 575 | 577 | 584 | 585 | 586 | 589 | 590 | 592 | 593 | 595 | 597 | 598 | 599 | 600 | 603 | 604 | 606 | 607 | 608 | 609 | 612 | 615 | 617 | 619 | 621 | 626 | 627 | 629 | 630 | 633 | 635 | 642 | 643 | 644 | 647 | 648 | 650 | 651 | 653 | 655 | 656 | 657 | 658 | 661 | 662 | 664 | 665 | 666 | 667 | 670 | 673 | 675 | 677 | 679 | 684 | 685 | 687 | 688 | 691 | 693 | 700 | 701 | 702 | 705 | 706 | 708 | 709 | 711 | 713 | 714 | 715 | 716 | 719 | 720 | 722 | 723 | 724 | 725 | 728 | 731 | 733 | 735 | 737 | 742 | 743 | 745 | 746 | 749 | 751 | 758 | 759 | 760 | 763 | 764 | 766 | 767 | 769 | 771 | 772 | 773 | 774 | 777 | 778 | 780 | 781 | 782 | 783 | 786 | 789 | 791 | 793 | 795 | 800 | 801 | 803 | 804 | 807 | 809 | 816 | 817 | 818 | 821 | 822 | 824 | 825 | 827 | 829 | 830 | 831 | 832 | 835 | 836 | 838 | 839 | 840 | 841 | 844 | 847 | 849 | 851 | 853 | 858 | 859 | 861 | 862 | 865 | 867 | 874 | 875 | 876 | 879 | 880 | 882 | 883 | 885 | 887 | 888 | 889 | 890 | 893 | 894 | 896 | 897 | 898 | 899 | 902 | 905 | 907 | 909 | 911 | 916 | 917 | 919 | 920 | 923 | 925 | 932 | 933 | 934 | 937 | 938 | 940 | 941 | 943 | 945 | 946 | 947 | 948 | 951 | 952 | 954 | 955 | 956 | 957 | 960 | 963 | 965 | 967 | 969 | 974 | 975 | 977 | 978 | 981 | 983 | 990 | 991 | 992 | 995 | 996 | 998 | 999 | 1001 | 1003 | 1004 | 1005 | 1006 | 1009 | 1010 | 1012 | 1013 | 1014 | 1015 | 1018 | 1021 | 1023 | 1025 | 1027 | 1032 | 1033 | 1035 | 1036 | 1039 | 1041 | 1048 | 1049 | 1050 | 1053 | 1054 | 1056 | 1057 | 1059 | 1061 | 1062 | 1063 | 1064 | 1067 | 1068 | 1070 | 1071 | 1072 | 1073 | 1076 | 1079 | 1081 | 1083 | 1085 | 1090 | 1091 | 1093 | 1094 | 1097 | 1099 | 1106 | 1107 | 1108 | 1111 | 1112 | 1114 | 1115 | 1117 | 1119 | 1120 | 1121 | 1122 | 1125 | 1126 | 1128 | 1129 | 1130 | 1131 | 1134 | 1137 | 1139 | 1141 | 1143 | 1148 | 1149 | 1151 | 1152 | 1155 | 1157 | 1164 | 1165 | 1166 | 1169 | 1170 | 1172 | 1173 | 1175 | 1177 | 1178 | 1179 | 1180 | 1183 | 1184 | 1186 | 1187 | 1188 | 1189 | 1192 | 1195 | 1197 | 1199 | 1201 | 1206 | 1207 | 1209 | 1210 | 1213 | 1215 | 1222 | 1223 | 1224 | 1227 | 1228 | 1230 | 1231 | 1233 | 1235 | 1236 | 1237 | 1238 | 1241 | 1242 | 1244 | 1245 | 1246 | 1247 | 1250 | 1253 | 1255 | 1257 | 1259 | 1264 | 1265 | -------------------------------------------------------------------------------- /URDF/urdf/raybotacan22.urdf: -------------------------------------------------------------------------------- 1 | 2 | 5 | 7 | 8 | 10 | 11 | 14 | 16 | 23 | 24 | 25 | 28 | 29 | 31 | 32 | 34 | 36 | 37 | 38 | 39 | 42 | 43 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 124 | 125 | 128 | 130 | 137 | 138 | 139 | 142 | 143 | 145 | 146 | 148 | 150 | 151 | 152 | 153 | 156 | 157 | 159 | 160 | 161 | 162 | 165 | 168 | 170 | 172 | 174 | 179 | 180 | 182 | 183 | 186 | 188 | 195 | 196 | 197 | 200 | 201 | 203 | 204 | 206 | 208 | 209 | 210 | 211 | 214 | 215 | 217 | 218 | 219 | 220 | 223 | 226 | 228 | 230 | 232 | 237 | 238 | 240 | 241 | 244 | 246 | 253 | 254 | 255 | 258 | 259 | 261 | 262 | 264 | 266 | 267 | 268 | 269 | 272 | 273 | 275 | 276 | 277 | 278 | 281 | 284 | 286 | 288 | 290 | 295 | 296 | 298 | 299 | 302 | 304 | 311 | 312 | 313 | 316 | 317 | 319 | 320 | 322 | 324 | 325 | 326 | 327 | 330 | 331 | 333 | 334 | 335 | 336 | 339 | 342 | 344 | 346 | 348 | 353 | 354 | 356 | 357 | 360 | 362 | 369 | 370 | 371 | 374 | 375 | 377 | 378 | 380 | 382 | 383 | 384 | 385 | 388 | 389 | 391 | 392 | 393 | 394 | 397 | 400 | 402 | 404 | 406 | 411 | 412 | 414 | 415 | 418 | 420 | 427 | 428 | 429 | 432 | 433 | 435 | 436 | 438 | 440 | 441 | 442 | 443 | 446 | 447 | 449 | 450 | 451 | 452 | 455 | 458 | 460 | 462 | 464 | 469 | 470 | 472 | 473 | 476 | 478 | 485 | 486 | 487 | 490 | 491 | 493 | 494 | 496 | 498 | 499 | 500 | 501 | 504 | 505 | 507 | 508 | 509 | 510 | 513 | 516 | 518 | 520 | 522 | 527 | 528 | 530 | 531 | 534 | 536 | 543 | 544 | 545 | 548 | 549 | 551 | 552 | 554 | 556 | 557 | 558 | 559 | 562 | 563 | 565 | 566 | 567 | 568 | 571 | 574 | 576 | 578 | 580 | 585 | 586 | 588 | 589 | 592 | 594 | 601 | 602 | 603 | 606 | 607 | 609 | 610 | 612 | 614 | 615 | 616 | 617 | 620 | 621 | 623 | 624 | 625 | 626 | 629 | 632 | 634 | 636 | 638 | 643 | 644 | 646 | 647 | 650 | 652 | 659 | 660 | 661 | 664 | 665 | 667 | 668 | 670 | 672 | 673 | 674 | 675 | 678 | 679 | 681 | 682 | 683 | 684 | 687 | 690 | 692 | 694 | 696 | 701 | 702 | 704 | 705 | 708 | 710 | 717 | 718 | 719 | 722 | 723 | 725 | 726 | 728 | 730 | 731 | 732 | 733 | 736 | 737 | 739 | 740 | 741 | 742 | 745 | 748 | 750 | 752 | 754 | 759 | 760 | 762 | 763 | 766 | 768 | 775 | 776 | 777 | 780 | 781 | 783 | 784 | 786 | 788 | 789 | 790 | 791 | 794 | 795 | 797 | 798 | 799 | 800 | 803 | 806 | 808 | 810 | 812 | 817 | 818 | 820 | 821 | 824 | 826 | 833 | 834 | 835 | 838 | 839 | 841 | 842 | 844 | 846 | 847 | 848 | 849 | 852 | 853 | 855 | 856 | 857 | 858 | 861 | 864 | 866 | 868 | 870 | 875 | 876 | 878 | 879 | 882 | 884 | 891 | 892 | 893 | 896 | 897 | 899 | 900 | 902 | 904 | 905 | 906 | 907 | 910 | 911 | 913 | 914 | 915 | 916 | 919 | 922 | 924 | 926 | 928 | 933 | 934 | 936 | 937 | 940 | 942 | 949 | 950 | 951 | 954 | 955 | 957 | 958 | 960 | 962 | 963 | 964 | 965 | 968 | 969 | 971 | 972 | 973 | 974 | 977 | 980 | 982 | 984 | 986 | 991 | 992 | 994 | 995 | 998 | 1000 | 1007 | 1008 | 1009 | 1012 | 1013 | 1015 | 1016 | 1018 | 1020 | 1021 | 1022 | 1023 | 1026 | 1027 | 1029 | 1030 | 1031 | 1032 | 1035 | 1038 | 1040 | 1042 | 1044 | 1049 | 1050 | 1052 | 1053 | 1056 | 1058 | 1065 | 1066 | 1067 | 1070 | 1071 | 1073 | 1074 | 1076 | 1078 | 1079 | 1080 | 1081 | 1084 | 1085 | 1087 | 1088 | 1089 | 1090 | 1093 | 1096 | 1098 | 1100 | 1102 | 1107 | 1108 | 1110 | 1111 | 1114 | 1116 | 1123 | 1124 | 1125 | 1128 | 1129 | 1131 | 1132 | 1134 | 1136 | 1137 | 1138 | 1139 | 1142 | 1143 | 1145 | 1146 | 1147 | 1148 | 1151 | 1154 | 1156 | 1158 | 1160 | 1165 | 1166 | 1168 | 1169 | 1172 | 1174 | 1181 | 1182 | 1183 | 1186 | 1187 | 1189 | 1190 | 1192 | 1194 | 1195 | 1196 | 1197 | 1200 | 1201 | 1203 | 1204 | 1205 | 1206 | 1209 | 1212 | 1214 | 1216 | 1218 | 1223 | 1224 | 1226 | 1227 | 1230 | 1232 | 1239 | 1240 | 1241 | 1244 | 1245 | 1247 | 1248 | 1250 | 1252 | 1253 | 1254 | 1255 | 1258 | 1259 | 1261 | 1262 | 1263 | 1264 | 1267 | 1270 | 1272 | 1274 | 1276 | 1281 | 1282 | 1284 | 1285 | 1288 | 1290 | 1297 | 1298 | 1299 | 1302 | 1303 | 1305 | 1306 | 1308 | 1310 | 1311 | 1312 | 1313 | 1316 | 1317 | 1319 | 1320 | 1321 | 1322 | 1325 | 1328 | 1330 | 1332 | 1334 | 1339 | 1340 | 1341 | -------------------------------------------------------------------------------- /Webots/translation_controller.py: -------------------------------------------------------------------------------- 1 | """keyboard_teleop controller.""" 2 | 3 | # You may need to import some classes of the controller module. Ex: 4 | # from controller import Robot, Motor, DistanceSensor 5 | from controller import Robot,Keyboard, Supervisor 6 | 7 | # create the Robot and supervisor instance. 8 | robot = Robot() 9 | _supervisor = Supervisor() 10 | 11 | robot_node = _supervisor.getFromDef('humanoid') 12 | transl_feild = robot_node.getField('translation') 13 | rotation_feild = robot_node.getField('rotation') 14 | # get the time step of the current world. 15 | TIME_STEP = 32 16 | 17 | kb=Keyboard() 18 | kb.enable(TIME_STEP) 19 | 20 | # Left arm motor list 21 | left_arm_motors = [] 22 | right_arm_motors = [] 23 | left_arm_motor_pos = [] 24 | right_arm_motor_pos = [] 25 | 26 | left_arm_names= ['il1','il2','il3','il4','il5','il6','il7','il8',] 27 | right_arm_names = ['ir1','ir2','ir3','ir4','ir5','ir6','ir7','ir8',] 28 | 29 | torso_motor_name = 'ia2' 30 | torso_motor = robot.getDevice(torso_motor_name) 31 | # Set initial velocities and positions to zero 32 | for i in range(8): 33 | left_arm_motors.append(robot.getDevice(left_arm_names[i])) 34 | right_arm_motors.append(robot.getDevice(right_arm_names[i])) 35 | 36 | 37 | 38 | 39 | # flags 40 | key_enable = True 41 | move_robot = False 42 | way_point = None 43 | #coounter 44 | i = 0 45 | new_transl = [0, 0, 0] 46 | 47 | 48 | while robot.step(TIME_STEP) != -1: 49 | key=kb.getKey() 50 | # print(key) 51 | 52 | if (key == 65) and key_enable: 53 | print("Movement request sent") 54 | key_enable = False 55 | move_robot = True 56 | way_point = "A" 57 | present_transl = robot_node.getPosition() 58 | i = 0 59 | elif (key== 66) and key_enable: 60 | print("Movement request sent") 61 | key_enable = False 62 | move_robot = True 63 | way_point = "B" 64 | present_transl = robot_node.getPosition() 65 | i = 0 66 | elif (key== 67) and key_enable: 67 | print("Movement request sent") 68 | key_enable = False 69 | move_robot = True 70 | way_point = "C" 71 | present_transl = robot_node.getPosition() 72 | i = 0 73 | elif (key== 68) and key_enable: 74 | print("Movement request sent") 75 | key_enable = False 76 | move_robot = True 77 | way_point = "D" 78 | present_orient = [0,0,0,1] 79 | i = 0 80 | elif (key== 69) and key_enable: 81 | print("Movement request sent") 82 | key_enable = False 83 | move_robot = True 84 | way_point = "E" 85 | present_orient = [0,0,-0.69,0.69] 86 | i = 0 87 | elif (key==Keyboard.UP) and key_enable: 88 | # torso motor device 89 | 90 | print("Moving to first pose") 91 | # #move torso motor to 20 degrees 92 | torso_motor.setPosition(-0.3) 93 | #set positon one for left_arm 94 | left_arm_motors[0].setPosition(-1.36) 95 | left_arm_motors[1].setPosition(-0.27) 96 | left_arm_motors[2].setPosition(-1.80) 97 | left_arm_motors[3].setPosition(-0.30) 98 | left_arm_motors[4].setPosition(-0.17) 99 | left_arm_motors[5].setPosition(1.25) 100 | #Set postion one for right_arm 101 | right_arm_motors[0].setPosition(1.36) 102 | right_arm_motors[1].setPosition(0.27) 103 | right_arm_motors[2].setPosition(1.80) 104 | right_arm_motors[3].setPosition(0.30) 105 | right_arm_motors[4].setPosition(0.17) 106 | right_arm_motors[5].setPosition(-1.25) 107 | print("Movement finish") 108 | elif (key==Keyboard.DOWN) and key_enable: 109 | print("Moving to second pose") 110 | torso_motor.setPosition(-0.95) 111 | #set positon one for left_arm 112 | left_arm_motors[0].setPosition(-1.36) 113 | left_arm_motors[1].setPosition(-0.27) 114 | left_arm_motors[2].setPosition(-1.80) 115 | left_arm_motors[3].setPosition(-0.30) 116 | left_arm_motors[4].setPosition(-0.17) 117 | left_arm_motors[5].setPosition(0.55) 118 | #Set postion one for right_arm 119 | right_arm_motors[0].setPosition(1.36) 120 | right_arm_motors[1].setPosition(0.27) 121 | right_arm_motors[2].setPosition(1.80) 122 | right_arm_motors[3].setPosition(0.30) 123 | right_arm_motors[4].setPosition(0.17) 124 | right_arm_motors[5].setPosition(-0.55) 125 | print("Movement finish") 126 | elif (key==Keyboard.LEFT) and key_enable: 127 | print("Moving to second pose") 128 | torso_motor.setPosition(0) 129 | #set positon one for left_arm 130 | left_arm_motors[0].setPosition(0) 131 | left_arm_motors[1].setPosition(0) 132 | left_arm_motors[2].setPosition(0) 133 | left_arm_motors[3].setPosition(0) 134 | left_arm_motors[4].setPosition(0) 135 | left_arm_motors[5].setPosition(0) 136 | #Set postion one for right_arm 137 | right_arm_motors[0].setPosition(0) 138 | right_arm_motors[1].setPosition(0) 139 | right_arm_motors[2].setPosition(0) 140 | right_arm_motors[3].setPosition(0) 141 | right_arm_motors[4].setPosition(0) 142 | right_arm_motors[5].setPosition(0) 143 | print("Movement finish") 144 | else : 145 | pass 146 | 147 | 148 | 149 | if move_robot: 150 | if way_point == "A" and i<30: 151 | print("Moving robot to point A") 152 | new_transl = present_transl 153 | new_transl[0] -=0.01 154 | transl_feild.setSFVec3f(new_transl) 155 | elif way_point == "B" and i<345: 156 | print("Moving robot to point B") 157 | new_transl = present_transl 158 | new_transl[1] -=0.01 159 | transl_feild.setSFVec3f(new_transl) 160 | elif way_point == "C" and i<30: 161 | print("Moving robot to point C") 162 | new_transl = present_transl 163 | new_transl[0] +=0.01 164 | transl_feild.setSFVec3f(new_transl) 165 | elif way_point == "D" and i<60: 166 | print("Moving robot to point D") 167 | present_orient[2] -=0.01 168 | present_orient[3] +=0.01 169 | rotation_feild.setSFRotation(present_orient) 170 | elif way_point == "E" and i<64: 171 | print("Moving robot to point E",i) 172 | present_orient[2] +=0.01 173 | present_orient[3] -=0.01 174 | rotation_feild.setSFRotation(present_orient) 175 | else: 176 | key_enable = True 177 | i += 1 178 | 179 | # Enter here exit cleanup code. 180 | -------------------------------------------------------------------------------- /中控.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Motor Control 6 | 28 | 29 | 30 |

设备控制

31 |
32 |
33 |

左手 (IP: 192.168.1.200)

34 | 35 | 36 | 37 | 38 |
39 | 40 | 41 | 42 |
43 |
44 | 45 |
46 | 47 | 48 |
49 |
50 |
51 |
52 | 72 |
73 |
74 |
75 |

同时控制 (左手 & 右手)

76 | 77 | 78 | 79 | 80 | 81 |
82 | 83 |
84 |
左手数据显示区域
85 |
右手数据显示区域
86 | 87 | 88 | 328 | 329 | 330 | --------------------------------------------------------------------------------