├── .github └── FUNDING.yml ├── LICENSE ├── Mainboard_version_2 ├── Mainboard_version_2.ino └── motor_reader.ino ├── README.md └── Robot_Program ├── Enable2.png ├── Estop2.png ├── Info_folder └── versions_info ├── Multi_proc_main.py ├── Programs ├── CSR_2.txt ├── CSR_BIG_SPEED.txt ├── CSSAR_TEST_DOBAR.txt ├── GRIPPER_TEST.txt ├── MOVE_123.txt ├── NEDIRAJ.txt ├── execute_script.txt ├── test.txt ├── test1.txt └── test123.txt ├── REAL_JTRAJ_SIM.py ├── Software_tests ├── CSAAR_test.py ├── JTRAJ_test.py ├── ctraj_test.py └── trajectory_tests.py ├── __pycache__ ├── axes_robot.cpython-36.pyc ├── get_send_data.cpython-36.pyc └── plot_graph_2.cpython-36.pyc ├── axes_robot.py ├── blue_arrow_left.png ├── blue_arrow_right.png ├── ctraj_test.py ├── disable_img.png ├── enable_img.png ├── get_send_data.py ├── helpimg4.png ├── plot_graph_2.py ├── plot_test.py ├── python_tests.py ├── robotic_toolbox_CM6_models ├── CM6.py └── __init__.py ├── xdole.png ├── xgore.png ├── ydesno.png ├── ylevo.png ├── zdole.png └── zgore.png /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | 3 | github: PCrnjak 4 | patreon: PCrnjak 5 | open_collective: # Replace with a single Open Collective username 6 | ko_fi: # Replace with a single Ko-fi username 7 | tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel 8 | community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry 9 | liberapay: # Replace with a single Liberapay username 10 | issuehunt: # Replace with a single IssueHunt username 11 | otechie: # Replace with a single Otechie username 12 | lfx_crowdfunding: # Replace with a single LFX Crowdfunding project-name e.g., cloud-foundry 13 | custom: # Replace with up to 4 custom sponsorship URLs e.g., ['link1', 'link2'] 14 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Petar Crnjak 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 | -------------------------------------------------------------------------------- /Mainboard_version_2/Mainboard_version_2.ino: -------------------------------------------------------------------------------- 1 | /************************************************************************* 2 | License GPL-3.0 3 | Copyright (c) 2020 Petar Crnjak 4 | License file : 5 | https://github.com/PCrnjak/S_Drive---small-BLDC-driver/blob/master/LICENSE 6 | **************************************************************************/ 7 | 8 | /************************************************************************* 9 | Created by: Petar Crnjak 10 | Version: 1.0 11 | Date: 21.3.2021. 12 | **************************************************************************/ 13 | 14 | /************************************************************************* 15 | https://www.pjrc.com/store/teensy41.html Datasheet for teensy 4.1 16 | This code runs on motherboard of CM series of robotic arms. 17 | Some of the functions of this code are: 18 | * Act as a point where all Driver data is being sent and and then passed to PC thru only one serial line. 19 | * So in CM6 example we have 6 joints and each is connected to one of 6 serial ports of teensy 4.1 ( of 8) 20 | Drivers send data every 10 ms and this code then packs data from all 6 joints and sends them thru USB to master PC. 21 | * Act as a safety mechanism in case of errors, driver disconnects, Estpo triggers, external interrupts, temperature errors 22 | 23 | **************************************************************************/ 24 | 25 | 26 | 27 | // IMPORTANT CHANGE THIS TO THE NUMBER OF JOINTS OF YOUR ROBOT!!! 28 | # define Joint_num 6 29 | 30 | int motor[Joint_num][6]; // Here is stored data from "Get_input" functions. Stored as they come: 31 | // data in order: position, current, speed, temperature, voltage, error. 32 | // data is stored as int values 33 | 34 | byte data_coming = 0; // Indicates if data is comming or not. 0 for not comming 1 for comming. 35 | /***************************************************************************** 36 | Use dummy data to test if you dont have drivers available 37 | data in order: position,current,speed,temperature,voltage,error 38 | ******************************************************************************/ 39 | int dummy_motor[6][6] = {{1000, 100, 10, 15, 1500, 1}, {2000, 200, 20, 25, 2400, 1}, {3000, 300, 30, 35, 3400, 1}, {4000, 400, 40, 45, 4500, 1}, {5000, 500, 50, 55, 5400, 1}, {6000, 600, 67, 65, 6500, 1}}; 40 | 41 | 42 | 43 | void setup() { 44 | pinMode(LED_BUILTIN, OUTPUT); 45 | 46 | // Begin serial communications 47 | Serial.begin(10000000); 48 | Serial1.begin(1000000); 49 | Serial2.begin(1000000); 50 | Serial3.begin(1000000); 51 | Serial4.begin(1000000); 52 | Serial5.begin(1000000); 53 | Serial6.begin(1000000); 54 | //Serial7.begin(1000000); 55 | //Serial8.begin(1000000); 56 | 57 | // Check why i did this ?!?! xD 58 | Serial.setTimeout(40); 59 | delay(100); 60 | 61 | Serial1.print("b"); Serial1.print("\n"); 62 | delay(300); 63 | Serial2.print("b"); Serial2.print("\n"); 64 | delay(300); 65 | Serial3.print("b"); Serial3.print("\n"); 66 | delay(300); 67 | Serial4.print("b"); Serial4.print("\n"); 68 | delay(300); 69 | Serial5.print("b"); Serial5.print("\n"); 70 | delay(300); 71 | Serial6.print("b"); Serial6.print("\n"); 72 | 73 | // mozda ga sjebu oni # koji pošalje ili motor setup stringovi 74 | // Whitout this it does not work so just leave it 75 | // Startup delay reads motor data for x micro seconds but does not forward it to usb serial. 76 | // Without this data gets corrupted or does not even work so leave it atm until this bug is fixed. 77 | // Possible solutions: 78 | // * drivers send # after we send b so maybe that breaks it ? 79 | // * possible motor setup strings that guide us thru setup ? 80 | startup_delay(1500000); 81 | 82 | } 83 | 84 | void loop() { 85 | 86 | // Gets motor data and save to motor[x] variable 87 | Get_input1(Serial1, motor[0]); 88 | Get_input2(Serial2, motor[1]); 89 | Get_input3(Serial3, motor[2]); 90 | Get_input4(Serial4, motor[3]); 91 | Get_input5(Serial5, motor[4]); 92 | Get_input6(Serial6, motor[5]); 93 | 94 | // Pack all received data from Get_input functions and send to PC serial! 95 | Motors_data_pack_send(20000); 96 | 97 | // Unpack data we received from PC serial and send it to specific driver! 98 | PC_data_unpack_send(); 99 | 100 | // Check if we are connected to PC serial. We need to get any data usefull data every 10 ms from PC. 101 | // If we dont get that data from PC for 60ms signal that we are no longer connected and it is error. 102 | check_if_connected(80000); //60ms 103 | 104 | } 105 | 106 | 107 | /***************************************************************************** 108 | Pin definitions 109 | They are specific for S-drive V3 dont change them. 110 | ******************************************************************************/ 111 | 112 | // Do this every x ms !! 113 | // Send data from all motors to PC every 10ms 114 | void Motors_data_pack_send(uint32_t sample_time) { 115 | 116 | static uint32_t previousMicros_ = 0; 117 | uint32_t currentMicros_ = micros(); 118 | int voltage_mean = 0; 119 | 120 | if (currentMicros_ - previousMicros_ >= sample_time) { 121 | previousMicros_ = currentMicros_; 122 | 123 | // i = rows(data elements), j = columns(motor number) 124 | for (int i = 0; i < 4 ; i++) { // only 4 here since we are sending position, speed, current and temperature 125 | for (int j = 0; j < Joint_num ; j++) { 126 | Serial.print(motor[j][i]); 127 | Serial.print(","); 128 | 129 | } 130 | } 131 | 132 | for (int i = 0; i < Joint_num ; i++) { 133 | voltage_mean = motor[i][4] + voltage_mean; 134 | } 135 | 136 | voltage_mean = voltage_mean / 3; 137 | Serial.print(voltage_mean); 138 | Serial.print(","); 139 | int Error_temp = 0; 140 | Serial.print(Error_temp); 141 | Serial.print("\n"); 142 | 143 | } 144 | } 145 | 146 | 147 | /***************************************************************************** 148 | Pin definitions 149 | They are specific for S-drive V3 dont change them. 150 | ******************************************************************************/ 151 | 152 | // Geta data from PC and send it to motors instantly 153 | void PC_data_unpack_send() { 154 | 155 | if (Serial.available()) { 156 | data_coming = 1; 157 | digitalWrite(LED_BUILTIN, LOW); 158 | 159 | String PC_out_string = Serial.readStringUntil('\n'); // Read string until '\n' 160 | int Joint_to_send = PC_out_string[1] - '0'; // This stuff converts ASCII number in this case PC_out_string[1] to int 161 | PC_out_string.remove(1, 1); // Remove joint number from our data string to prepare it to be forwarded to appropriate joint 162 | 163 | switch (Joint_to_send) { 164 | case 1: //Joint 1 165 | Serial1.print(PC_out_string); 166 | Serial1.print('\n'); 167 | break; 168 | case 2: //Joint 2 169 | Serial2.print(PC_out_string); 170 | Serial2.print('\n'); 171 | break; 172 | case 3: //Joint 3 173 | Serial3.print(PC_out_string); 174 | Serial3.print('\n'); 175 | break; 176 | case 4: //Joint 4 177 | Serial4.print(PC_out_string); 178 | Serial4.print('\n'); 179 | break; 180 | case 5: //Joint 5 181 | Serial5.print(PC_out_string); 182 | Serial5.print('\n'); 183 | break; 184 | case 6: //Joint 6 185 | Serial6.print(PC_out_string); 186 | Serial6.print('\n'); 187 | // 188 | break; 189 | 190 | } 191 | } 192 | } 193 | 194 | 195 | /***************************************************************************** 196 | Pin definitions 197 | They are specific for S-drive V3 dont change them. 198 | ******************************************************************************/ 199 | 200 | void check_if_connected(uint32_t sample_time) { 201 | 202 | static uint32_t previousMicros_ = 0; 203 | uint32_t currentMicros_ = micros(); 204 | 205 | if (data_coming == 1) { 206 | previousMicros_ = currentMicros_; 207 | 208 | } 209 | 210 | if (currentMicros_ - previousMicros_ >= sample_time) { 211 | previousMicros_ = currentMicros_; 212 | digitalWrite(LED_BUILTIN, HIGH); 213 | } 214 | data_coming = 0; 215 | 216 | 217 | } 218 | 219 | 220 | 221 | /***************************************************************************** 222 | Pin definitions 223 | They are specific for S-drive V3 dont change them. 224 | ******************************************************************************/ 225 | 226 | void startup_delay(uint32_t sample_time) { 227 | static bool var = 0; 228 | while (var == 0) { 229 | static uint32_t previousMicros_c = 0; 230 | uint32_t currentMicros_c = micros(); 231 | Get_input1(Serial1, motor[0]); 232 | Get_input2(Serial2, motor[1]); 233 | Get_input3(Serial3, motor[2]); /// motor 2? 234 | Get_input4(Serial4, motor[3]); 235 | Get_input5(Serial5, motor[4]); 236 | Get_input6(Serial6, motor[5]); /// motor 2? 237 | if (currentMicros_c - previousMicros_c >= sample_time) { 238 | previousMicros_c = currentMicros_c; 239 | var = 1; 240 | } 241 | } 242 | } 243 | -------------------------------------------------------------------------------- /Mainboard_version_2/motor_reader.ino: -------------------------------------------------------------------------------- 1 | void Get_input1(Stream &serialport, int *data_) { 2 | 3 | static uint8_t i_ = 0; 4 | static uint8_t i2 = 0; 5 | static char data[30]; 6 | static char data2[30]; 7 | static uint8_t comma = 1; 8 | static bool flag_2 = 0; 9 | 10 | // 6 vars i svi su int 11 | static int out1_ ; 12 | static int out2_ ; 13 | static int out3_ ; 14 | static int out4_ ; 15 | static int out5_ ; 16 | static int out6_ ; 17 | 18 | 19 | if (serialport.available() > 0) { 20 | 21 | data[i_] = serialport.read(); 22 | 23 | if (data[i_] == 't') { 24 | flag_2 = 1; 25 | } 26 | if ( data[i_] == '\n') { 27 | flag_2 = 0; 28 | } 29 | 30 | if (i_ != 0) { 31 | switch (data[0]) { 32 | 33 | /***************************************************************************** 34 | GOTO position and hold (with error comp) 35 | ******************************************************************************/ 36 | case 't': 37 | data2[i2 - 1] = data[i_]; 38 | 39 | if (data[i_] == '\n' and comma == 6) { 40 | data[i_] = '\0'; 41 | data2[i2 - 1] = data[i_]; 42 | out6_ = atoi(data2); 43 | data_[5] = out6_; 44 | data_[4] = out5_; 45 | data_[3] = out4_; 46 | data_[2] = out3_; 47 | data_[1] = out2_; 48 | data_[0] = out1_; 49 | 50 | //Serial.println(Position_var); 51 | comma = 1 ; 52 | i2 = 0; 53 | i_ = 0; 54 | 55 | } 56 | 57 | if (data[i_] == ',' and comma == 5) { 58 | data[i_] = '\0'; 59 | data2[i2 - 1] = data[i_]; 60 | out5_ = atoi(data2); 61 | comma = comma + 1 ; 62 | i2 = 0; 63 | } 64 | 65 | 66 | if (data[i_] == ',' and comma == 4) { 67 | data[i_] = '\0'; 68 | data2[i2 - 1] = data[i_]; 69 | out4_ = atoi(data2); 70 | comma = comma + 1 ; 71 | i2 = 0; 72 | } 73 | 74 | 75 | if (data[i_] == ',' and comma == 3) { 76 | data[i_] = '\0'; 77 | data2[i2 - 1] = data[i_]; 78 | out3_ = atoi(data2); 79 | comma = comma + 1 ; 80 | i2 = 0; 81 | } 82 | 83 | if (data[i_] == ',' and comma == 2) { 84 | data[i_] = '\0'; 85 | data2[i2 - 1] = data[i_]; 86 | out2_ = atoi(data2); 87 | comma = comma + 1 ; 88 | i2 = 0; 89 | } 90 | 91 | 92 | if (data[i_] == ',' and comma == 1) { 93 | data[i_] = '\0'; 94 | data2[i2 - 1] = data[i_]; 95 | out1_ = atoi(data2); 96 | comma = comma + 1 ; 97 | i2 = 0; 98 | } 99 | 100 | break; 101 | 102 | } 103 | } 104 | if (flag_2 == 1) { 105 | i_ = i_ + 1; 106 | i2 = i2 + 1; 107 | } 108 | if (flag_2 == 0) { 109 | i_ = 0; 110 | i2 = 0; 111 | comma = 1 ; 112 | } 113 | 114 | 115 | } 116 | } 117 | 118 | void Get_input2(Stream &serialport, int *data_) { 119 | 120 | static uint8_t i_ = 0; 121 | static uint8_t i2 = 0; 122 | static char data[30]; 123 | static char data2[30]; 124 | static uint8_t comma = 1; 125 | static bool flag_2 = 0; 126 | 127 | // 6 vars i svi su int 128 | static int out1_ = 0; 129 | static int out2_ = 0; 130 | static int out3_ = 0; 131 | static int out4_ = 0; 132 | static int out5_ = 0; 133 | static int out6_ = 0; 134 | 135 | 136 | if (serialport.available() > 0) { 137 | 138 | data[i_] = serialport.read(); 139 | 140 | if (data[i_] == 't') { 141 | flag_2 = 1; 142 | } 143 | if ( data[i_] == '\n') { 144 | flag_2 = 0; 145 | } 146 | 147 | if (i_ != 0) { 148 | switch (data[0]) { 149 | 150 | /***************************************************************************** 151 | GOTO position and hold (with error comp) 152 | ******************************************************************************/ 153 | case 't': 154 | data2[i2 - 1] = data[i_]; 155 | 156 | if (data[i_] == '\n' and comma == 6) { 157 | data[i_] = '\0'; 158 | data2[i2 - 1] = data[i_]; 159 | out6_ = atoi(data2); 160 | 161 | data_[5] = out6_; 162 | data_[4] = out5_; 163 | data_[3] = out4_; 164 | data_[2] = out3_; 165 | data_[1] = out2_; 166 | data_[0] = out1_; 167 | comma = 1 ; 168 | i2 = 0; 169 | i_ = 0; 170 | 171 | } 172 | 173 | if (data[i_] == ',' and comma == 5) { 174 | data[i_] = '\0'; 175 | data2[i2 - 1] = data[i_]; 176 | out5_ = atoi(data2); 177 | comma = comma + 1 ; 178 | i2 = 0; 179 | } 180 | 181 | 182 | if (data[i_] == ',' and comma == 4) { 183 | data[i_] = '\0'; 184 | data2[i2 - 1] = data[i_]; 185 | out4_ = atoi(data2); 186 | comma = comma + 1 ; 187 | i2 = 0; 188 | } 189 | 190 | 191 | if (data[i_] == ',' and comma == 3) { 192 | data[i_] = '\0'; 193 | data2[i2 - 1] = data[i_]; 194 | out3_ = atoi(data2); 195 | comma = comma + 1 ; 196 | i2 = 0; 197 | } 198 | 199 | if (data[i_] == ',' and comma == 2) { 200 | data[i_] = '\0'; 201 | data2[i2 - 1] = data[i_]; 202 | out2_ = atoi(data2); 203 | comma = comma + 1 ; 204 | i2 = 0; 205 | } 206 | 207 | 208 | if (data[i_] == ',' and comma == 1) { 209 | data[i_] = '\0'; 210 | data2[i2 - 1] = data[i_]; 211 | out1_ = atoi(data2); 212 | comma = comma + 1 ; 213 | i2 = 0; 214 | } 215 | 216 | break; 217 | 218 | } 219 | } 220 | if (flag_2 == 1) { 221 | i_ = i_ + 1; 222 | i2 = i2 + 1; 223 | } 224 | if (flag_2 == 0) { 225 | i_ = 0; 226 | i2 = 0; 227 | comma = 1 ; 228 | } 229 | 230 | 231 | } 232 | } 233 | 234 | void Get_input3(Stream &serialport, int *data_) { 235 | 236 | static uint8_t i_ = 0; 237 | static uint8_t i2 = 0; 238 | static char data[30]; 239 | static char data2[30]; 240 | static uint8_t comma = 1; 241 | static bool flag_2 = 0; 242 | 243 | // 6 vars i svi su int 244 | static int out1_ = 0; 245 | static int out2_ = 0; 246 | static int out3_ = 0; 247 | static int out4_ = 0; 248 | static int out5_ = 0; 249 | static int out6_ = 0; 250 | 251 | 252 | if (serialport.available() > 0) { 253 | 254 | data[i_] = serialport.read(); 255 | 256 | if (data[i_] == 't') { 257 | flag_2 = 1; 258 | } 259 | if ( data[i_] == '\n') { 260 | flag_2 = 0; 261 | } 262 | 263 | if (i_ != 0) { 264 | switch (data[0]) { 265 | 266 | case 't': 267 | data2[i2 - 1] = data[i_]; 268 | 269 | if (data[i_] == '\n' and comma == 6) { 270 | data[i_] = '\0'; 271 | data2[i2 - 1] = data[i_]; 272 | out6_ = atoi(data2); 273 | 274 | data_[5] = out6_; 275 | data_[4] = out5_; 276 | data_[3] = out4_; 277 | data_[2] = out3_; 278 | data_[1] = out2_; 279 | data_[0] = out1_; 280 | //Serial.println(Position_var); 281 | comma = 1 ; 282 | i2 = 0; 283 | i_ = 0; 284 | 285 | } 286 | 287 | if (data[i_] == ',' and comma == 5) { 288 | data[i_] = '\0'; 289 | data2[i2 - 1] = data[i_]; 290 | out5_ = atoi(data2); 291 | comma = comma + 1 ; 292 | i2 = 0; 293 | } 294 | 295 | 296 | if (data[i_] == ',' and comma == 4) { 297 | data[i_] = '\0'; 298 | data2[i2 - 1] = data[i_]; 299 | out4_ = atoi(data2); 300 | comma = comma + 1 ; 301 | i2 = 0; 302 | } 303 | 304 | 305 | if (data[i_] == ',' and comma == 3) { 306 | data[i_] = '\0'; 307 | data2[i2 - 1] = data[i_]; 308 | out3_ = atoi(data2); 309 | comma = comma + 1 ; 310 | i2 = 0; 311 | } 312 | 313 | if (data[i_] == ',' and comma == 2) { 314 | data[i_] = '\0'; 315 | data2[i2 - 1] = data[i_]; 316 | out2_ = atoi(data2); 317 | comma = comma + 1 ; 318 | i2 = 0; 319 | } 320 | 321 | 322 | if (data[i_] == ',' and comma == 1) { 323 | data[i_] = '\0'; 324 | data2[i2 - 1] = data[i_]; 325 | out1_ = atoi(data2); 326 | comma = comma + 1 ; 327 | i2 = 0; 328 | } 329 | 330 | break; 331 | 332 | } 333 | } 334 | if (flag_2 == 1) { 335 | i_ = i_ + 1; 336 | i2 = i2 + 1; 337 | } 338 | if (flag_2 == 0) { 339 | i_ = 0; 340 | i2 = 0; 341 | comma = 1 ; 342 | } 343 | 344 | 345 | } 346 | } 347 | 348 | 349 | void Get_input4(Stream &serialport, int *data_) { 350 | 351 | static uint8_t i_ = 0; 352 | static uint8_t i2 = 0; 353 | static char data[30]; 354 | static char data2[30]; 355 | static uint8_t comma = 1; 356 | static bool flag_2 = 0; 357 | 358 | // 6 vars i svi su int 359 | static int out1_ ; 360 | static int out2_ ; 361 | static int out3_ ; 362 | static int out4_ ; 363 | static int out5_ ; 364 | static int out6_ ; 365 | 366 | 367 | if (serialport.available() > 0) { 368 | 369 | data[i_] = serialport.read(); 370 | 371 | if (data[i_] == 't') { 372 | flag_2 = 1; 373 | } 374 | if ( data[i_] == '\n') { 375 | flag_2 = 0; 376 | } 377 | 378 | if (i_ != 0) { 379 | switch (data[0]) { 380 | 381 | /***************************************************************************** 382 | GOTO position and hold (with error comp) 383 | ******************************************************************************/ 384 | case 't': 385 | data2[i2 - 1] = data[i_]; 386 | 387 | if (data[i_] == '\n' and comma == 6) { 388 | data[i_] = '\0'; 389 | data2[i2 - 1] = data[i_]; 390 | out6_ = atoi(data2); 391 | data_[5] = out6_; 392 | data_[4] = out5_; 393 | data_[3] = out4_; 394 | data_[2] = out3_; 395 | data_[1] = out2_; 396 | data_[0] = out1_; 397 | 398 | //Serial.println(Position_var); 399 | comma = 1 ; 400 | i2 = 0; 401 | i_ = 0; 402 | 403 | } 404 | 405 | if (data[i_] == ',' and comma == 5) { 406 | data[i_] = '\0'; 407 | data2[i2 - 1] = data[i_]; 408 | out5_ = atoi(data2); 409 | comma = comma + 1 ; 410 | i2 = 0; 411 | } 412 | 413 | 414 | if (data[i_] == ',' and comma == 4) { 415 | data[i_] = '\0'; 416 | data2[i2 - 1] = data[i_]; 417 | out4_ = atoi(data2); 418 | comma = comma + 1 ; 419 | i2 = 0; 420 | } 421 | 422 | 423 | if (data[i_] == ',' and comma == 3) { 424 | data[i_] = '\0'; 425 | data2[i2 - 1] = data[i_]; 426 | out3_ = atoi(data2); 427 | comma = comma + 1 ; 428 | i2 = 0; 429 | } 430 | 431 | if (data[i_] == ',' and comma == 2) { 432 | data[i_] = '\0'; 433 | data2[i2 - 1] = data[i_]; 434 | out2_ = atoi(data2); 435 | comma = comma + 1 ; 436 | i2 = 0; 437 | } 438 | 439 | 440 | if (data[i_] == ',' and comma == 1) { 441 | data[i_] = '\0'; 442 | data2[i2 - 1] = data[i_]; 443 | out1_ = atoi(data2); 444 | comma = comma + 1 ; 445 | i2 = 0; 446 | } 447 | 448 | break; 449 | 450 | } 451 | } 452 | if (flag_2 == 1) { 453 | i_ = i_ + 1; 454 | i2 = i2 + 1; 455 | } 456 | if (flag_2 == 0) { 457 | i_ = 0; 458 | i2 = 0; 459 | comma = 1 ; 460 | } 461 | 462 | 463 | } 464 | } 465 | 466 | void Get_input5(Stream &serialport, int *data_) { 467 | 468 | static uint8_t i_ = 0; 469 | static uint8_t i2 = 0; 470 | static char data[30]; 471 | static char data2[30]; 472 | static uint8_t comma = 1; 473 | static bool flag_2 = 0; 474 | 475 | // 6 vars i svi su int 476 | static int out1_ ; 477 | static int out2_ ; 478 | static int out3_ ; 479 | static int out4_ ; 480 | static int out5_ ; 481 | static int out6_ ; 482 | 483 | 484 | if (serialport.available() > 0) { 485 | 486 | data[i_] = serialport.read(); 487 | 488 | if (data[i_] == 't') { 489 | flag_2 = 1; 490 | } 491 | if ( data[i_] == '\n') { 492 | flag_2 = 0; 493 | } 494 | 495 | if (i_ != 0) { 496 | switch (data[0]) { 497 | 498 | /***************************************************************************** 499 | GOTO position and hold (with error comp) 500 | ******************************************************************************/ 501 | case 't': 502 | data2[i2 - 1] = data[i_]; 503 | 504 | if (data[i_] == '\n' and comma == 6) { 505 | data[i_] = '\0'; 506 | data2[i2 - 1] = data[i_]; 507 | out6_ = atoi(data2); 508 | data_[5] = out6_; 509 | data_[4] = out5_; 510 | data_[3] = out4_; 511 | data_[2] = out3_; 512 | data_[1] = out2_; 513 | data_[0] = out1_; 514 | 515 | //Serial.println(Position_var); 516 | comma = 1 ; 517 | i2 = 0; 518 | i_ = 0; 519 | 520 | } 521 | 522 | if (data[i_] == ',' and comma == 5) { 523 | data[i_] = '\0'; 524 | data2[i2 - 1] = data[i_]; 525 | out5_ = atoi(data2); 526 | comma = comma + 1 ; 527 | i2 = 0; 528 | } 529 | 530 | 531 | if (data[i_] == ',' and comma == 4) { 532 | data[i_] = '\0'; 533 | data2[i2 - 1] = data[i_]; 534 | out4_ = atoi(data2); 535 | comma = comma + 1 ; 536 | i2 = 0; 537 | } 538 | 539 | 540 | if (data[i_] == ',' and comma == 3) { 541 | data[i_] = '\0'; 542 | data2[i2 - 1] = data[i_]; 543 | out3_ = atoi(data2); 544 | comma = comma + 1 ; 545 | i2 = 0; 546 | } 547 | 548 | if (data[i_] == ',' and comma == 2) { 549 | data[i_] = '\0'; 550 | data2[i2 - 1] = data[i_]; 551 | out2_ = atoi(data2); 552 | comma = comma + 1 ; 553 | i2 = 0; 554 | } 555 | 556 | 557 | if (data[i_] == ',' and comma == 1) { 558 | data[i_] = '\0'; 559 | data2[i2 - 1] = data[i_]; 560 | out1_ = atoi(data2); 561 | comma = comma + 1 ; 562 | i2 = 0; 563 | } 564 | 565 | break; 566 | 567 | } 568 | } 569 | if (flag_2 == 1) { 570 | i_ = i_ + 1; 571 | i2 = i2 + 1; 572 | } 573 | if (flag_2 == 0) { 574 | i_ = 0; 575 | i2 = 0; 576 | comma = 1 ; 577 | } 578 | 579 | 580 | } 581 | } 582 | 583 | void Get_input6(Stream &serialport, int *data_) { 584 | 585 | static uint8_t i_ = 0; 586 | static uint8_t i2 = 0; 587 | static char data[30]; 588 | static char data2[30]; 589 | static uint8_t comma = 1; 590 | static bool flag_2 = 0; 591 | 592 | // 6 vars i svi su int 593 | static int out1_ ; 594 | static int out2_ ; 595 | static int out3_ ; 596 | static int out4_ ; 597 | static int out5_ ; 598 | static int out6_ ; 599 | 600 | 601 | if (serialport.available() > 0) { 602 | 603 | data[i_] = serialport.read(); 604 | 605 | if (data[i_] == 't') { 606 | flag_2 = 1; 607 | } 608 | if ( data[i_] == '\n') { 609 | flag_2 = 0; 610 | } 611 | 612 | if (i_ != 0) { 613 | switch (data[0]) { 614 | 615 | /***************************************************************************** 616 | GOTO position and hold (with error comp) 617 | ******************************************************************************/ 618 | case 't': 619 | data2[i2 - 1] = data[i_]; 620 | 621 | if (data[i_] == '\n' and comma == 6) { 622 | data[i_] = '\0'; 623 | data2[i2 - 1] = data[i_]; 624 | out6_ = atoi(data2); 625 | data_[5] = out6_; 626 | data_[4] = out5_; 627 | data_[3] = out4_; 628 | data_[2] = out3_; 629 | data_[1] = out2_; 630 | data_[0] = out1_; 631 | 632 | //Serial.println(Position_var); 633 | comma = 1 ; 634 | i2 = 0; 635 | i_ = 0; 636 | 637 | } 638 | 639 | if (data[i_] == ',' and comma == 5) { 640 | data[i_] = '\0'; 641 | data2[i2 - 1] = data[i_]; 642 | out5_ = atoi(data2); 643 | comma = comma + 1 ; 644 | i2 = 0; 645 | } 646 | 647 | 648 | if (data[i_] == ',' and comma == 4) { 649 | data[i_] = '\0'; 650 | data2[i2 - 1] = data[i_]; 651 | out4_ = atoi(data2); 652 | comma = comma + 1 ; 653 | i2 = 0; 654 | } 655 | 656 | 657 | if (data[i_] == ',' and comma == 3) { 658 | data[i_] = '\0'; 659 | data2[i2 - 1] = data[i_]; 660 | out3_ = atoi(data2); 661 | comma = comma + 1 ; 662 | i2 = 0; 663 | } 664 | 665 | if (data[i_] == ',' and comma == 2) { 666 | data[i_] = '\0'; 667 | data2[i2 - 1] = data[i_]; 668 | out2_ = atoi(data2); 669 | comma = comma + 1 ; 670 | i2 = 0; 671 | } 672 | 673 | 674 | if (data[i_] == ',' and comma == 1) { 675 | data[i_] = '\0'; 676 | data2[i2 - 1] = data[i_]; 677 | out1_ = atoi(data2); 678 | comma = comma + 1 ; 679 | i2 = 0; 680 | } 681 | 682 | break; 683 | 684 | } 685 | } 686 | if (flag_2 == 1) { 687 | i_ = i_ + 1; 688 | i2 = i2 + 1; 689 | } 690 | if (flag_2 == 0) { 691 | i_ = 0; 692 | i2 = 0; 693 | comma = 1 ; 694 | } 695 | 696 | 697 | } 698 | } 699 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![License: MIT](https://img.shields.io/badge/License-MIT-green.svg)](https://opensource.org/licenses/MIT) 2 | 3 | Join discord community: https://discord.com/invite/prjUvjmGpZ 4 | # Dependancy: 5 | 6 | Tested on Ubuntu 18.04.5 LTS running on virtual machine 7 | 8 | Running Python 3.6.9 (default, Jan 26 2021, 15:33:00) 9 | 10 | Robotic toolbox version - Downloaded 9.3.2021 - Realease 6 v0.9.1 [Toolbox](https://github.com/petercorke/robotics-toolbox-python) 11 | 12 | For additional python packages check /info_folder/versions_info 13 | 14 | # Installation steps 15 | 16 | After installation of Robotic toolbox you will need to add: 17 | 18 | __init__.py and CM6.py from robotic_toolbox_CM6_models folder to installation folder of robotic toolbox library: 19 | 20 | ...robotic-toolbox-python/robotictoolbox/models/DH 21 | 22 | * After everything is installed when you run Multi_proc_main.py you should get a screen as shown on first image. If you get error for this step: in get_send_data.py file comment s.open() part of code. 23 | * To run this code in tandem with your robot you will need to modify get_send_data.py to your COM port. And you will need to be running mainboard code on your MCU. 24 | * Mainboard code is configured for teensy 4.1. 25 | * Connection diagram and more info can be found here: - [Main CM6 project page](https://github.com/PCrnjak/CM6_COBOT_ROBOT) 26 | 27 | 28 | # CM6_control_software 29 | 30 | CM6_control_software allows "easy" programming of CM6 robot. GUI software was written in python and heavily relies on Peter Corke's robotic toolbox for python! It was tested on Linux virtual machine, laptop running Linux, and raspberry pi 4! 31 | 32 | The software offers real-time monitoring of robots: 33 | 34 | * Motor position, current, speed, temperature 35 | * End effector position 36 | * Operating modes, errors... 37 | * 38 | Available modes at this moment are: 39 | 40 | * Individual motor jogging 41 | * freehand teach 42 | * move from point to point 43 | 44 | Each of these modes of movement can be recorded and replayed! 45 | 46 | 47 | 48 | 49 | 50 | # Support the project 51 | 52 | This project is completely Open source and free to all and I would like to keep it that way, so any help 53 | in terms of donations or advice is really appreciated. Thank you! 54 | 55 | [![Check the arm in action !](https://user-images.githubusercontent.com/30388414/86798915-a036ba00-c071-11ea-824d-4456f2cdf797.png)](https://paypal.me/PCrnjak?locale.x=en_US) 56 | 57 | # Project is under MIT Licence 58 | -------------------------------------------------------------------------------- /Robot_Program/Enable2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/Enable2.png -------------------------------------------------------------------------------- /Robot_Program/Estop2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/Estop2.png -------------------------------------------------------------------------------- /Robot_Program/Info_folder/versions_info: -------------------------------------------------------------------------------- 1 | Tested on Ubuntu 18.04.5 LTS running on virtual machine 2 | 3 | Running Python 3.6.9 (default, Jan 26 2021, 15:33:00) 4 | 5 | Robotic toolbox version - Downloaded 9.3.2021 - Realease 6 v0.9.1 6 | 7 | 8 | 9 | Versions of all installed packages: $pip3 list 10 | ansitable (0.9.5) 11 | apturl (0.5.2) 12 | argon2-cffi (20.1.0) 13 | asn1crypto (0.24.0) 14 | astroid (2.4.2) 15 | async-generator (1.10) 16 | attrs (20.2.0) 17 | backcall (0.2.0) 18 | bleach (3.2.1) 19 | Brlapi (0.6.6) 20 | certifi (2020.6.20) 21 | cffi (1.14.3) 22 | chardet (3.0.4) 23 | colored (1.4.2) 24 | command-not-found (0.3) 25 | cryptography (2.1.4) 26 | cupshelpers (1.0) 27 | cycler (0.10.0) 28 | Cython (0.29.22) 29 | decorator (4.4.2) 30 | defer (1.0.6) 31 | defusedxml (0.6.0) 32 | distro-info (0.18ubuntu0.18.04.1) 33 | entrypoints (0.3) 34 | future (0.18.2) 35 | httplib2 (0.9.2) 36 | idna (2.10) 37 | ikpy (3.0.1) 38 | importlib-metadata (2.0.0) 39 | ipykernel (5.3.4) 40 | ipython (7.16.1) 41 | ipython-genutils (0.2.0) 42 | isort (5.5.4) 43 | jedi (0.17.2) 44 | Jinja2 (2.11.2) 45 | json5 (0.9.5) 46 | jsonschema (3.2.0) 47 | jupyter-client (6.1.7) 48 | jupyter-core (4.6.3) 49 | jupyterlab (2.2.8) 50 | jupyterlab-pygments (0.1.2) 51 | jupyterlab-server (1.2.0) 52 | keyring (10.6.0) 53 | keyrings.alt (3.0) 54 | kiwisolver (1.3.1) 55 | language-selector (0.1) 56 | launchpadlib (1.10.6) 57 | lazr.restfulclient (0.13.5) 58 | lazr.uri (1.0.3) 59 | lazy-object-proxy (1.4.3) 60 | louis (3.5.0) 61 | macaroonbakery (1.1.3) 62 | Mako (1.0.7) 63 | MarkupSafe (1.1.1) 64 | matplotlib (3.3.4) 65 | mccabe (0.6.1) 66 | mistune (0.8.4) 67 | mpmath (1.1.0) 68 | nbclient (0.5.1) 69 | nbconvert (6.0.7) 70 | nbformat (5.0.8) 71 | nest-asyncio (1.4.1) 72 | netifaces (0.10.4) 73 | notebook (6.1.4) 74 | numpy (1.19.5) 75 | oauth (1.0.1) 76 | olefile (0.45.1) 77 | packaging (20.4) 78 | pandocfilters (1.4.2) 79 | parso (0.7.1) 80 | pexpect (4.8.0) 81 | pgraph-python (0.6.1) 82 | pickleshare (0.7.5) 83 | Pillow (8.1.2) 84 | pip (9.0.1) 85 | prometheus-client (0.8.0) 86 | prompt-toolkit (3.0.8) 87 | protobuf (3.0.0) 88 | ptyprocess (0.6.0) 89 | pycairo (1.16.2) 90 | pycparser (2.20) 91 | pycrypto (2.6.1) 92 | pycups (1.9.73) 93 | Pygments (2.7.1) 94 | pygobject (3.26.1) 95 | pylint (2.6.0) 96 | pymacaroons (0.13.0) 97 | PyNaCl (1.1.2) 98 | pyparsing (2.4.7) 99 | pyRFC3339 (1.0) 100 | pyrsistent (0.17.3) 101 | pyserial (3.4) 102 | python-apt (1.6.5+ubuntu0.5) 103 | python-dateutil (2.8.1) 104 | python-debian (0.1.32) 105 | pytz (2018.3) 106 | pyxdg (0.25) 107 | PyYAML (3.12) 108 | pyzmq (19.0.2) 109 | qpsolvers (1.5) 110 | quadprog (0.1.8) 111 | reportlab (3.4.0) 112 | requests (2.24.0) 113 | requests-unixsocket (0.1.5) 114 | roboticstoolbox-python (0.9.1) 115 | rtb-data (0.9) 116 | scipy (1.5.4) 117 | SecretStorage (2.3.1) 118 | Send2Trash (1.5.0) 119 | setuptools (50.3.2) 120 | simplejson (3.13.2) 121 | six (1.15.0) 122 | spatialmath-python (0.9.2) 123 | swift-sim (0.8.1) 124 | sympy (1.6.2) 125 | system-service (0.3) 126 | systemd-python (234) 127 | terminado (0.9.1) 128 | testpath (0.4.4) 129 | tkintertable (1.3.2) 130 | toml (0.10.1) 131 | tornado (6.0.4) 132 | traitlets (4.3.3) 133 | typed-ast (1.4.1) 134 | ubuntu-drivers-common (0.0.0) 135 | ufw (0.36) 136 | unattended-upgrades (0.1) 137 | urllib3 (1.25.11) 138 | usb-creator (0.3.3) 139 | wadllib (1.3.2) 140 | wcwidth (0.2.5) 141 | webencodings (0.5.1) 142 | websockets (8.1) 143 | wheel (0.30.0) 144 | wrapt (1.12.1) 145 | xkit (0.0.0) 146 | zipp (3.3.1) 147 | zope.interface (4.3.2) 148 | -------------------------------------------------------------------------------- /Robot_Program/Multi_proc_main.py: -------------------------------------------------------------------------------- 1 | 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import tkinter as tk 5 | from tkinter.ttk import Progressbar 6 | from tkinter import filedialog 7 | from PIL import Image, ImageTk 8 | import multiprocessing 9 | import serial as sr 10 | import time 11 | import random 12 | import numpy as np 13 | import get_send_data as gt 14 | import os,os.path 15 | import axes_robot as rbt 16 | import roboticstoolbox as rp 17 | #from spatialmath import * 18 | import plot_graph_2 as plots 19 | 20 | Image_path = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(__file__))) 21 | print(Image_path) 22 | 23 | current_frame = 0 # tells us what frame is active. Move, log, teach... 24 | 25 | acc_set = 45 #preset to some value these are % from main to max acc 26 | spd_set = 25 #preset to some value these are % from main to max speed 27 | 28 | Enable_disable = False # True is for operating / False is for Stoped 29 | 30 | gravity_disable_var = True # True is for gravity comp mode / False is for disabled 31 | 32 | Execute_stop_var = True # True is for stop / False is for execute 33 | 34 | Now_open = '' 35 | 36 | robot_arm = rp.models.DH.CM6() 37 | 38 | # p1 = Speed_setpoint 39 | # p2 = acc_setpoint 40 | # p3 = translations 41 | # p4 = left_btns 42 | # p5 = right_btns 43 | # p6 = motor_positions(encoder_ticks) 44 | # p7 = Real RAD angle 45 | # p8 = Current 46 | # p9 = Temperature 47 | # p10 = Robot_pose 48 | # p11 = grav_pos_flag 49 | # p12 = software_control_variables 50 | 51 | # p13 = Steer_K1 52 | # p14 = Steer_K2 53 | # p15 = Steer_K3 54 | # p16 = Steer_K4 55 | 56 | def Tkinter_GUI(p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11,p12,p13,p14,p15,p16): 57 | 58 | # When button is pressed raise selected frame 59 | def raise_frame(frame,button1,button2,button3,button4,name): 60 | 61 | global current_frame 62 | current_frame = name 63 | if name == 'move': 64 | p12[1] = 0 65 | if name == 'log': 66 | p12[1] = 1 67 | if name == 'teach': 68 | p12[1] = 2 69 | if name == 'setup': 70 | p12[1] = 3 71 | 72 | # https://www.tutorialspoint.com/python/tk_relief.htm 73 | button1.config(relief='sunken',bg = main_color) #,borderwidth=3 74 | button2.config(relief='raised',bg = "white") 75 | button3.config(relief='raised',bg = "white") 76 | button4.config(relief='raised',bg = "white") 77 | 78 | frame.tkraise() 79 | 80 | def move_frame_stuff(): 81 | 82 | ################ Speed and acceleration setup canvas ######### 83 | 84 | # This segment creates speed and acceleration controls 85 | # They are adjusted by % and shown in RPM and RAD/S² for single joint movement 86 | # and in m/s and m/s² for translation movements 87 | 88 | speed_canvas = tk.Canvas(move_frame, width=1400, height=80,bg = "white",borderwidth=6, relief='ridge') 89 | speed_canvas.place(x = 0, y = 0) 90 | 91 | # This is used to show how fast roboto will move when performing translational motion or rotation around its axes 92 | speed_label = tk.Label(move_frame, text="Speed settings" ,font = (letter_font,18),bg = "white") 93 | speed_label.place(x = 10, y = 10) 94 | 95 | set_speed_RPM = tk.Label(move_frame, text="####" ,font = (letter_font,14),bg = "white") 96 | set_speed_RPM .place(x = 500, y = 10) 97 | 98 | set_speed_RAD = tk.Label(move_frame, text="####" ,font = (letter_font,14),bg = "white") 99 | set_speed_RAD .place(x = 500, y = 35) 100 | 101 | set_ACC_RAD = tk.Label(move_frame, text="####" ,font = (letter_font,14),bg = "white") 102 | set_ACC_RAD .place(x = 500, y = 60) 103 | 104 | 105 | # Set speed and acceleration when pressing buttons 106 | def speed_acc_setup(var): 107 | if var == 0: 108 | p1.value = 25 # p1 is speed 109 | p2.value = 40 110 | 111 | elif var == 1: 112 | p1.value = 50 113 | p2.value = 50 114 | 115 | elif var == 2: 116 | p1.value = 80 117 | p2.value = 55 118 | 119 | # Ovo će biti za translacije i rotacije 120 | #set_speed_RPM.configure(text = str(p1.value) + " RPM") 121 | #set_speed_RAD.configure(text = str(p1.value) + " RAD/S") 122 | #set_ACC_RAD.configure(text = str(p2.value) + " RAD/S²") 123 | 124 | # This updates values for current desired speed and acceleration in GUI 125 | for y in range(0,rbt.Joint_num): 126 | btns_rads[y].configure(text = "Speed: " + str(round(np.interp(p1.value,[1,100],[rbt.Joint_min_speed[y],rbt.Joint_max_speed[y]]),4)) + " RAD/S") 127 | btns_accel[y].configure(text = "Acceleration: " + str(round(np.interp(p2.value,[1,100],[rbt.Joint_min_acc[y],rbt.Joint_max_acc[y]]),4)) + " RAD/S²") 128 | 129 | 130 | 131 | # Set Speed and acc with sliders 132 | def set_speed_acc(): 133 | p1.value = spd_set # speed 134 | p2.value = acc_set # acceleration 135 | 136 | # This updates values for current desired speed and acceleration in GUI 137 | for y in range(0,rbt.Joint_num): 138 | btns_rads[y].configure(text = "Speed: " + str(round(np.interp(p1.value,[1,100],[rbt.Joint_min_speed[y],rbt.Joint_max_speed[y]]),4)) + " RAD/S") 139 | btns_accel[y].configure(text = "Acceleration: " + str(round(np.interp(p2.value,[1,100],[rbt.Joint_min_acc[y],rbt.Joint_max_acc[y]]),4)) + " RAD/S²") 140 | 141 | # Ovo će biti za translacije i rotacije 142 | #set_speed_RPM.configure(text = str(round(rbt.RADS2_true_RPM(var_),4)) + " RPM") 143 | #set_speed_RAD.configure(text = str(round(var_,4)) + " RAD/S") 144 | #set_ACC_RAD.configure(text = str(round(var2_,4)) + " RAD/S²") 145 | 146 | # Button for slow speed 147 | spd_b_1 = tk.Button(move_frame, text = "Slow",bg = "white", font = (letter_font,18), width = 10, height = 1,borderwidth=3,command = lambda:speed_acc_setup(0)) 148 | spd_b_1.place(x = 195-180, y = 40) 149 | 150 | # Button for default speed 151 | spd_b_2 = tk.Button(move_frame, text = "Default",bg = "white", font = (letter_font,18), width = 10, height = 1,borderwidth=3,command = lambda:speed_acc_setup(1)) 152 | spd_b_2.place(x = 345-180, y = 40) 153 | 154 | # Button for fast speed 155 | spd_b_2 = tk.Button(move_frame, text = "Fast",bg = "white", font = (letter_font,18), width = 10, height = 1,borderwidth=3,command = lambda:speed_acc_setup(2)) 156 | spd_b_2.place(x = 495-180, y = 40) 157 | 158 | # Button to set speed from sliders 159 | set_btn = tk.Button(move_frame, text = "Set",bg = "white", font = (letter_font,18), width = 3, height = 1,borderwidth=3,command = lambda:set_speed_acc()) 160 | set_btn.place(x = 1320, y = 25) 161 | 162 | ################ Motor jog canvas ############################ 163 | 164 | jog_motors_canvas = tk.Canvas(move_frame, width=700, height=800,bg = "white",borderwidth=6, relief='ridge') 165 | jog_motors_canvas.place(x = 0, y = 100) 166 | 167 | btns_left = [] 168 | btns_right = [] 169 | btns_label = [] 170 | btns_rads = [] 171 | btns_accel = [] 172 | 173 | btn_nr = -1 174 | 175 | global tk_left 176 | image_left = Image.open(os.path.join(Image_path,'blue_arrow_left.png')) 177 | tk_left = ImageTk.PhotoImage(image_left) 178 | 179 | global tk_right 180 | image_right = Image.open(os.path.join(Image_path,'blue_arrow_right.png')) 181 | tk_right = ImageTk.PhotoImage(image_right) 182 | 183 | robot_names = ['Base', 'Shoulder', 'Elbow', 'Wrist 1', 'Wrist 2', 'Wrist 3'] 184 | 185 | def button_press_left(event=None, var = 0): 186 | p4[var] = 1 187 | 188 | def button_rel_left(event=None, var = 0): 189 | p4[var] = 0 190 | 191 | def button_press_right(event=None, var = 0): 192 | p5[var] = 1 193 | 194 | def button_rel_right(event=None, var = 0): 195 | p5[var] = 0 196 | 197 | # https://stackoverflow.com/questions/14259072/tkinter-bind-function-with-variable-in-a-loop 198 | 199 | for y in range(1,7): 200 | 201 | btn_nr += 1 202 | 203 | def make_lambda1(x): 204 | return lambda ev:button_press_left(ev,x) 205 | 206 | def make_lambda2(x): 207 | return lambda ev:button_rel_left(ev,x) 208 | 209 | def make_lambda3(x): 210 | return lambda ev:button_press_right(ev,x) 211 | 212 | def make_lambda4(x): 213 | return lambda ev:button_rel_right(ev,x) 214 | 215 | # Create buttons for joging motors and labels for speed and acceleration 216 | btns_label.append(tk.Label(move_frame, text=robot_names[btn_nr] ,font = (letter_font,16),bg = "white")) 217 | btns_label[btn_nr].place(x = 17, y = 130+btn_nr*140) 218 | 219 | btns_left.append(tk.Button(move_frame,image = tk_left,bg ="white",highlightthickness = 0,borderwidth=0)) 220 | btns_left[btn_nr].place(x = 150, y = 118+btn_nr*135) 221 | btns_left[btn_nr].bind('',make_lambda1(btn_nr)) 222 | btns_left[btn_nr].bind('',make_lambda2(btn_nr)) 223 | 224 | btns_right.append(tk.Button(move_frame,image = tk_right,bg ="white",highlightthickness = 0,borderwidth=0)) 225 | btns_right[btn_nr].place(x = 610, y = 118+btn_nr*135) 226 | btns_right[btn_nr].bind('',make_lambda3(btn_nr)) 227 | btns_right[btn_nr].bind('',make_lambda4(btn_nr)) 228 | 229 | btns_rads.append(tk.Label(move_frame, text= "Speed: " ,font = (letter_font,8,'bold'),bg = "white")) 230 | btns_rads[btn_nr].place(x = 17, y = 165+btn_nr*140) 231 | 232 | btns_accel.append(tk.Label(move_frame, text= "Acceleration: " ,font = (letter_font,8,'bold'),bg = "white")) 233 | btns_accel[btn_nr].place(x = 17, y = 188+btn_nr*140) 234 | 235 | set_speed_acc() 236 | 237 | ################ Translation canvas ########################## 238 | jog_pose_canvas = tk.Canvas(move_frame, width=680, height=800,bg = "white",highlightthickness = 0,borderwidth=6, relief='ridge') 239 | jog_pose_canvas.place(x = 720, y = 100) 240 | 241 | global tk_xu 242 | global tk_xd 243 | global tk_yl 244 | global tk_yr 245 | global tk_zu 246 | global tk_zd 247 | 248 | ylevo = Image.open(os.path.join(Image_path,'ylevo.png')) 249 | tk_yl = ImageTk.PhotoImage(ylevo) 250 | 251 | ydesno = Image.open(os.path.join(Image_path,'ydesno.png')) 252 | tk_yr = ImageTk.PhotoImage(ydesno) 253 | 254 | xgore = Image.open(os.path.join(Image_path,'xgore.png')) 255 | tk_xu = ImageTk.PhotoImage(xgore) 256 | 257 | xdole = Image.open(os.path.join(Image_path,'xdole.png')) 258 | tk_xd = ImageTk.PhotoImage(xdole) 259 | 260 | zgore = Image.open(os.path.join(Image_path,'zgore.png')) 261 | tk_zu = ImageTk.PhotoImage(zgore) 262 | 263 | zdole = Image.open(os.path.join(Image_path,'zdole.png')) 264 | tk_zd = ImageTk.PhotoImage(zdole) 265 | 266 | 267 | translation_position = [] 268 | 269 | def translation_press(event=None,ax=0): 270 | p3[ax] = 1 271 | 272 | def translation_release(event=None,ax=0): 273 | p3[ax] = 0 274 | 275 | def make_lambda_press(x): 276 | return lambda ev:translation_press(ev,x) 277 | 278 | def make_lambda_release(x): 279 | return lambda ev:translation_release(ev,x) 280 | 281 | 282 | zu_button = tk.Button(move_frame, image=tk_zu,borderwidth=0,highlightthickness = 0,bg = 'white') 283 | zu_button.place(x = 1160, y = 180-60) 284 | zu_button.bind('',make_lambda_press(5)) 285 | zu_button.bind('', make_lambda_release(5)) 286 | 287 | zd_button = tk.Button(move_frame, image=tk_zd,borderwidth=0,highlightthickness = 0,bg = 'white') 288 | zd_button.place(x = 810, y = 180-60) 289 | zd_button.bind('',make_lambda_press(4)) 290 | zd_button.bind('', make_lambda_release(4)) 291 | 292 | yl_button = tk.Button(move_frame, image=tk_yl,borderwidth=0,highlightthickness = 0,bg = 'white') 293 | yl_button.place(x = 810, y = 440-140) 294 | yl_button.bind('',make_lambda_press(3)) 295 | yl_button.bind('', make_lambda_release(3)) 296 | 297 | yr_button = tk.Button(move_frame, image=tk_yr,borderwidth=0,highlightthickness = 0,bg = 'white') 298 | yr_button.place(x = 1160, y = 440-140) 299 | yr_button.bind('',make_lambda_press(2)) 300 | yr_button.bind('', make_lambda_release(2)) 301 | 302 | xu_button = tk.Button(move_frame, image=tk_xu,borderwidth=0,highlightthickness = 0,bg = 'white') 303 | xu_button.place(x = 1090-100, y = 400-140) 304 | xu_button.bind('',make_lambda_press(1)) 305 | xu_button.bind('', make_lambda_release(1)) 306 | 307 | xd_button = tk.Button(move_frame, image=tk_xd,borderwidth=0,highlightthickness = 0,bg = 'white') 308 | xd_button.place(x = 1060-100, y = 600-140) 309 | xd_button.bind('',make_lambda_press(0)) 310 | xd_button.bind('', make_lambda_release(0)) 311 | 312 | ############################################################## 313 | 314 | def log_frame_stuff(): 315 | # Here write code for log frame 316 | None 317 | 318 | def teach_frame_stuff(): 319 | 320 | gravity_hold_canvas_left = tk.Canvas(teach_frame, width=275, height=900,bg = "white",borderwidth=6, relief='ridge') 321 | gravity_hold_canvas_left.place(x = 0, y = 0) 322 | 323 | gravity_hold_canvas_right = tk.Canvas(teach_frame, width=275, height=900,bg = "white",borderwidth=6, relief='ridge') 324 | gravity_hold_canvas_right.place(x = 290, y = 0) 325 | 326 | save_canvas = tk.Canvas(teach_frame, width=550, height=280,bg = "white",borderwidth=6, relief='ridge') 327 | save_canvas.place(x = 855, y = 620) 328 | 329 | control_canvas_teach = tk.Canvas(teach_frame, width=265, height=900,bg = "white",borderwidth=6, relief='ridge') 330 | control_canvas_teach.place(x = 580, y = 0) 331 | 332 | gravity_l = tk.Label(teach_frame, text="Gravity compensation" ,font = (letter_font,17),bg = "white") 333 | gravity_l.place(x = 20, y = 10) 334 | 335 | position_l = tk.Label(teach_frame, text="Position hold" ,font = (letter_font,17),bg = "white") 336 | position_l.place(x = 310, y = 10) 337 | 338 | def open_txt(): 339 | 340 | global Now_open 341 | mytext.delete('1.0', tk.END) 342 | text_file = filedialog.askopenfilename(initialdir = Image_path + "/Programs",title = "open text file", filetypes = (("Text Files","*.txt"),)) 343 | print(text_file) 344 | Now_open = text_file 345 | text_file = open(text_file, 'r+') 346 | stuff = text_file.read() 347 | 348 | mytext.insert(tk.END,stuff) 349 | text_file.close() 350 | 351 | def save_txt(): 352 | 353 | global Now_open 354 | print(Now_open) 355 | if Now_open != '': 356 | print("done") 357 | text_file = open(Now_open,'w+') 358 | text_file.write(mytext.get(1.0,tk.END)) 359 | text_file.close() 360 | else: 361 | Now_open = Image_path + "/Programs/execute_script.txt" 362 | text_file = open(Now_open,'w+') 363 | text_file.write(mytext.get(1.0,tk.END)) 364 | text_file.close() 365 | 366 | def save_as_txt(): 367 | 368 | var1 = entry_label.get() 369 | text_file = open(Image_path + "/Programs/" + var1 +".txt",'w+') 370 | text_file.write(mytext.get(1.0,tk.END)) 371 | text_file.close() 372 | 373 | def record_position(mode_var): 374 | 375 | if entry_label_duration.get() == '': 376 | move_duration = str(4) 377 | else: 378 | move_duration = entry_label_duration.get() 379 | 380 | string = mode_var + ',' #'pos,' 381 | string = string + move_duration + ',' 382 | 383 | for y in range(0,rbt.Joint_num - 1): 384 | string = string + str(round(p7[y],5)) + ',' 385 | string = string + str(round(p7[rbt.Joint_num-1],5)) #add last joint without "," at the end 386 | string = string + ',\n' 387 | mytext.insert(tk.INSERT,string) 388 | 389 | def record_delay(): 390 | if entry_label_delay.get() == '': 391 | delay_time = 1.5 392 | else: 393 | delay_time = entry_label_delay.get() 394 | mytext.insert(tk.INSERT,'delay,') 395 | mytext.insert(tk.INSERT,str(delay_time)) 396 | mytext.insert(tk.INSERT,',\n') 397 | 398 | 399 | p12[3] = 0 400 | def execute_stuff(): 401 | 402 | global Now_open 403 | data2save = mytext.get(1.0,tk.END) 404 | p12[3] = 1 405 | if Now_open != '': 406 | text_file = open(Now_open,'w+') 407 | text_file.write(data2save) 408 | text_file.close() 409 | text_file = open(Image_path + "/Programs/execute_script.txt",'w+') 410 | text_file.write(data2save) 411 | text_file.close() 412 | 413 | else: 414 | 415 | Now_open = Image_path + "/Programs/execute_script.txt" 416 | text_file = open(Now_open,'w+') 417 | text_file.write(data2save) 418 | text_file.close() 419 | 420 | def stop_execution(): 421 | p12[3] = 0 422 | 423 | def pause_execution(): 424 | p12[3] = 2 425 | 426 | mytext = tk.Text(teach_frame,width = 55, height = 30, font=("Helvetica", 13), bg ='gray') 427 | mytext.place(x = 860, y = 10) 428 | 429 | execute_button = tk.Button(teach_frame,text = "Execute",font = (letter_font,22), width = 7, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3, command = execute_stuff) 430 | execute_button.place(x = 1250, y = 630) 431 | 432 | stop_execution_button = tk.Button(teach_frame,text = "Stop",font = (letter_font,22), width = 7, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3, command = stop_execution) 433 | stop_execution_button.place(x = 1250, y = 685) 434 | 435 | pause_execution_button = tk.Button(teach_frame,text = "Pause",font = (letter_font,22), width = 7, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3, command = pause_execution) 436 | pause_execution_button.place(x = 1250, y = 740) 437 | 438 | open_button = tk.Button(teach_frame,text = "Open",font = (letter_font,22), width = 7, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = open_txt) 439 | open_button.place(x = 1085, y = 630) 440 | 441 | save_button = tk.Button(teach_frame,text = "Save",font = (letter_font,14,'bold'), width = 6, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = save_txt) 442 | save_button.place(x = 865, y = 630) 443 | 444 | save_as_button = tk.Button(teach_frame,text = "Save as",font = (letter_font,14,'bold'), width = 6, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = save_as_txt) 445 | save_as_button.place(x = 975, y = 630) 446 | 447 | 448 | p12[4] = 0 449 | def Start_recording(): 450 | 451 | if p12[4] == 0: 452 | start_recording_button.configure(bg ="green") 453 | else: 454 | start_recording_button.configure(bg ="ivory3") 455 | p12[4] = not(p12[4]) 456 | 457 | 458 | def Stop_recording(): 459 | p12[4] = 0 460 | start_recording_button.configure(bg ="ivory3") 461 | 462 | p12[5] = 0 463 | #def Execute_recording(): 464 | #if p12[5] == 0: 465 | #execute_recording_button.configure(bg ="green") 466 | #else: 467 | #execute_recording_button.configure(bg ="ivory3") 468 | #p12[5] = not(p12[5]) 469 | def Execute_recording(): 470 | p12[5] = 1 471 | 472 | p12[6] = 0 473 | def Show_recording(): 474 | p12[6] = 1 475 | 476 | 477 | p12[7] = 0 478 | def Clear_recording(): 479 | p12[7] = 1 480 | 481 | start_recording_button = tk.Button(teach_frame,text = "Start REC",font = (letter_font,14), width = 9, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = Start_recording) 482 | start_recording_button.place(x =870, y = 710) 483 | 484 | stop_recording_button = tk.Button(teach_frame,text = "Stop REC",font = (letter_font,14), width = 9, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = Stop_recording) 485 | stop_recording_button.place(x =870, y = 750) 486 | 487 | execute_recording_button = tk.Button(teach_frame,text = "Execute REC",font = (letter_font,14), width = 9, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = Execute_recording) 488 | execute_recording_button.place(x =870, y = 790) 489 | 490 | show_recording_button = tk.Button(teach_frame,text = "Show REC",font = (letter_font,14), width = 9, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = Show_recording) 491 | show_recording_button.place(x =870, y = 830) 492 | 493 | Clear_recording_button = tk.Button(teach_frame,text = "Clear REC",font = (letter_font,14), width = 9, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = Clear_recording) 494 | Clear_recording_button.place(x =870, y = 870) 495 | 496 | entry_label = tk.Entry(teach_frame,font = (letter_font,14,'bold'), width = 15, borderwidth = 4,bg ="gray84") 497 | entry_label.place(x =870, y = 675) 498 | 499 | # Legacy stuff using GOTO position 500 | # Command is pos 501 | record_button_all = tk.Button(teach_frame,text = "Record all",font = (letter_font,19), width = 14, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = lambda: record_position('pos')) 502 | record_button_all.place(x =20, y = 810) 503 | 504 | record_button_free = tk.Button(teach_frame,text = "Record free",font = (letter_font,19), width = 14, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = lambda: record_position('pos')) 505 | record_button_free.place(x =20, y = 860) 506 | ################################################ 507 | 508 | CSAAR_button = tk.Button(teach_frame,text = "CSAAR",font = (letter_font,19), width = 14, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = lambda: record_position('CSAAR')) 509 | CSAAR_button.place(x =590, y = 70+43) 510 | 511 | JTRAJ_button = tk.Button(teach_frame,text = "JTRAJ",font = (letter_font,19), width = 14, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = lambda: record_position('JTRAJ')) 512 | JTRAJ_button.place(x =590, y = 70+90) 513 | 514 | CTRAJ_button = tk.Button(teach_frame,text = "CTRAJ",font = (letter_font,19), width = 14, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = lambda: record_position('CTRAJ')) 515 | CTRAJ_button.place(x =590, y = 70+90+47) 516 | 517 | move_duration_l = tk.Label(teach_frame, text="Move duration" ,font = (letter_font,17),bg ="ivory3",highlightthickness = 0,borderwidth=3) 518 | move_duration_l.place(x =590, y = 112-90) 519 | 520 | entry_label_duration = tk.Entry(teach_frame,font = (letter_font,19,'bold'), width = 4, borderwidth = 4,bg ="gray84") 521 | entry_label_duration.place(x =767, y = 110-90) 522 | 523 | delay_button = tk.Button(teach_frame,text = "Delay",font = (letter_font,19), width = 9, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = record_delay) 524 | delay_button.place(x =590, y = 155-90) 525 | 526 | entry_label_delay = tk.Entry(teach_frame,font = (letter_font,19,'bold'), width = 4, borderwidth = 4,bg ="gray84") 527 | entry_label_delay.place(x =767, y = 160-90) 528 | 529 | def loop_command(): 530 | mytext.insert(tk.INSERT,'loop,\n') 531 | 532 | def end_command(): 533 | mytext.insert(tk.INSERT,'end,\n') 534 | 535 | end_button = tk.Button(teach_frame,text ="END",font = (letter_font,19), width = 6, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = end_command) 536 | end_button.place(x =590, y = 855) 537 | 538 | loop_button = tk.Button(teach_frame,text = "LOOP",font = (letter_font,19), width = 6, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = loop_command) 539 | loop_button.place(x =720, y = 855) 540 | 541 | 542 | def button_press_grav(event=None, var = 0): 543 | grav_buttons[var].configure(bg ="green yellow") 544 | pos_buttons[var].configure(bg ="ivory3") 545 | p11[var] = 0 546 | 547 | def button_press_pos(event=None, var = 0): 548 | grav_buttons[var].configure(bg ="ivory3") 549 | pos_buttons[var].configure(bg ="green yellow") 550 | p11[var] = 1 551 | 552 | def set_all(var): 553 | if var == 'grav': 554 | for y in range(0,6): 555 | grav_buttons[y].configure(bg ="green yellow") 556 | pos_buttons[y].configure(bg ="ivory3") 557 | p11[y] = 0 558 | 559 | if var == 'pos': 560 | for y in range(0,6): 561 | grav_buttons[y].configure(bg ="ivory3") 562 | pos_buttons[y].configure(bg ="green yellow") 563 | p11[y] = 1 564 | 565 | 566 | def make_lambda_grav(x): 567 | return lambda ev:button_press_grav(ev,x) 568 | 569 | def make_lambda_pos(x): 570 | return lambda ev:button_press_pos(ev,x) 571 | 572 | robot_names_text = ['Base', 'Shoulder', 'Elbow', 'Wrist 1', 'Wrist 2', 'Wrist 3'] 573 | grav_buttons = [] 574 | pos_buttons = [] 575 | 576 | for cnt in range(0,6): 577 | 578 | grav_buttons.append(tk.Button(teach_frame,text = robot_names_text[cnt],font = (letter_font,22), width = 9, height = 1,bg ="ivory3",highlightthickness = 0,borderwidth=3)) 579 | grav_buttons[cnt].place(x = 7, y = 50+cnt*50) 580 | grav_buttons[cnt].bind('',make_lambda_grav(cnt)) 581 | 582 | pos_buttons.append(tk.Button(teach_frame,text = robot_names_text[cnt],font = (letter_font,22), width = 9, height = 1,bg ="green yellow",highlightthickness = 0,borderwidth=3)) 583 | pos_buttons[cnt].place(x = 300, y = 50+cnt*50) 584 | pos_buttons[cnt].bind('',make_lambda_pos(cnt)) 585 | 586 | grav_all=(tk.Button(teach_frame,text = "ALL",font = (letter_font,22), wraplength=1, width = 2, height = 8,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = lambda: set_all('grav'))) 587 | grav_all.place(x = 207, y = 50) 588 | 589 | pos_all=(tk.Button(teach_frame,text = "ALL",font = (letter_font,22), wraplength=1, width = 2, height = 8,bg ="ivory3",highlightthickness = 0,borderwidth=3,command = lambda: set_all('pos'))) 590 | pos_all.place(x = 500, y = 50) 591 | 592 | gravity_disable_var = 1 593 | p11[6] = gravity_disable_var 594 | 595 | def switch_grav_disable(): 596 | global gravity_disable_var 597 | if gravity_disable_var == 0: 598 | grav_disable.configure(text = "Disable") 599 | else: 600 | grav_disable.configure(text = "Gravity") 601 | gravity_disable_var = not(gravity_disable_var) 602 | p11[6] = gravity_disable_var 603 | 604 | grav_disable=(tk.Button(teach_frame,text = "Disable",font = (letter_font,22), width = 9, height = 1,bg ="ivory4",highlightthickness = 0,borderwidth=3,command = lambda: switch_grav_disable())) 605 | grav_disable.place(x = 7, y = 355) 606 | 607 | p12[2] = 1 608 | def set_grav_pos(): 609 | p12[2] = not(p12[2]) 610 | 611 | set_motor_mode=(tk.Button(teach_frame,text = "Set",font = (letter_font,22), width = 9, height = 1,bg ="ivory4",highlightthickness = 0,borderwidth=3,command = lambda: set_grav_pos())) 612 | set_motor_mode.place(x = 300, y = 355) 613 | 614 | None 615 | 616 | def setup_frame_stuff(): 617 | 618 | # Initialize K1,2,3,4 values to default ones 619 | for y in range(0,rbt.Joint_num): 620 | p13[y] = rbt.Steer_K1_default[y] 621 | p14[y] = rbt.Steer_K2_default[y] 622 | p15[y] = rbt.Steer_K3_default[y] 623 | p16[y] = rbt.Steer_K4_default[y] 624 | 625 | # When button is pressed change K1,2,3,4 values 626 | def Change_compliance(): 627 | comp_temp = round(np.interp(Compliance_scale.get(), [0, 100], [18, 0.2]),3) 628 | for y in range(0,rbt.Joint_num): 629 | p13[y] = comp_temp 630 | print(p13[y]) 631 | Current_compliance_label.configure(text="Current compliance K1 value: " + str(p13[0]) ) 632 | 633 | 634 | Compliance_setup_canvas = tk.Canvas(setup_frame, width=900, height=900,bg = "white",borderwidth=6, relief='ridge') 635 | Compliance_setup_canvas.place(x = 0, y = 0) 636 | 637 | Compliance_settings_l = tk.Label(setup_frame, text="Compliance settings" ,font = (letter_font,17),bg = "white") 638 | Compliance_settings_l.place(x = 20, y = 10) 639 | 640 | Compliance_scale = tk.Scale(setup_frame, label='Compliance in %. (100% being soft, 0 being stiff)', from_=0, to=100, orient = tk.HORIZONTAL,bg = "white",borderwidth=3,length = 400, font = (letter_font,11)) 641 | Compliance_scale.place(x = 23, y = 75) 642 | 643 | Compliance_set_button = tk.Button(setup_frame,text = "Set",font = (letter_font,20),bg = "ivory3",command = lambda:Change_compliance()) 644 | Compliance_set_button.place(x = 350, y = 15) 645 | 646 | Current_compliance_label = tk.Label(setup_frame, text="Current compliance K1 value: " + str(p13[0]), font = (letter_font,13),bg = "white") 647 | Current_compliance_label.place(x = 20, y = 47) 648 | 649 | 650 | letter_font = 'Courier New TUR' # letter font used 651 | # http://www.science.smith.edu/dftwiki/index.php/Color_Charts_for_TKinter 652 | main_color = 'gray63' # Color of background (This is some light blue color) 653 | root = tk.Tk() 654 | root.wm_attributes("-topmost", 0) 655 | root.title('ARM control') 656 | root.configure(background = main_color) 657 | 658 | # This maintains fixed size of 1920x1080 659 | # while enabling to minimise and maximise 660 | 661 | root.maxsize(1920,1080) 662 | root.minsize(1920,1080) 663 | 664 | root.geometry("1920x1080") 665 | 666 | # Create frames for other windows 667 | 668 | move_frame = tk.Frame(root, background = main_color) 669 | move_frame.place(x=0, y=85, width=1420, height=1010) 670 | 671 | log_frame = tk.Frame(root, background = main_color) 672 | log_frame.place(x=0, y=85, width=1420, height=1010) 673 | 674 | teach_frame = tk.Frame(root, background = main_color) 675 | teach_frame.place(x=0, y=85, width=1420, height=1010) 676 | 677 | setup_frame = tk.Frame(root, background = main_color) 678 | setup_frame.place(x=0, y=85, width=1420, height=1010) 679 | 680 | #Help image and button 681 | 682 | image_help = Image.open(os.path.join(Image_path,'helpimg4.png')) 683 | tk_image = ImageTk.PhotoImage(image_help) 684 | 685 | help_button = tk.Button(root, image=tk_image,borderwidth=0,highlightthickness = 0,bg = main_color) #, command=lambda aurl=url_donate:OpenUrl_donate(aurl) 686 | help_button.place(x = 1830, y = 0) 687 | 688 | # Buttons for window select 689 | 690 | control_canvas = tk.Canvas(root, width=470, height=900,bg = "white",borderwidth=6, relief='ridge') 691 | control_canvas.place(x = 1420, y = 85) 692 | 693 | positons_label = tk.Label(root, text="Tool position:" ,font = (letter_font,18),bg = "white") 694 | positons_label.place(x = 1450, y = 10+85) 695 | 696 | Enable_disable = 0 # 1 is for enabled, 0 is for disabled 697 | p12[0] = Enable_disable 698 | 699 | def Disable_Enable(): 700 | global Enable_disable 701 | if Enable_disable == 0: 702 | STOP_button.configure(image=tk_STOP) 703 | else: 704 | STOP_button.configure(image=tk_ENABLE) 705 | Enable_disable = not(Enable_disable) 706 | p12[0] = Enable_disable 707 | 708 | image_STOP = Image.open(os.path.join(Image_path,'disable_img.png')) 709 | image_ENABLE = Image.open(os.path.join(Image_path,'enable_img.png')) 710 | tk_STOP = ImageTk.PhotoImage(image_STOP) 711 | tk_ENABLE = ImageTk.PhotoImage(image_ENABLE) 712 | 713 | # Button to stop robot 714 | STOP_button = tk.Button(root, image=tk_ENABLE,borderwidth=0,highlightthickness = 0,bg = "white",command = lambda:Disable_Enable()) 715 | STOP_button.place(x = 1760, y = 30+85) 716 | 717 | # Button to clear error 718 | 719 | def Clear_error_command(): 720 | gt.Clear_Error(1) 721 | gt.Clear_Error(2) 722 | gt.Clear_Error(3) 723 | gt.Clear_Error(4) 724 | gt.Clear_Error(5) 725 | gt.Clear_Error(6) 726 | 727 | Clear_error_button = tk.Button(root,text = "Clear error",font = (letter_font,24),bg = "ivory3",command = Clear_error_command) 728 | Clear_error_button.place(x = 1450, y = 270+85) 729 | 730 | # Button to close gripper 731 | Gripper_close_button = tk.Button(root,text = "Close gripper",font = (letter_font,20),bg = "ivory3") 732 | Gripper_close_button.place(x = 1450, y = 370+85) 733 | 734 | # Button to open gripper 735 | Gripper_open_button = tk.Button(root,text = "Open gripper",font = (letter_font,20),bg = "ivory3") 736 | Gripper_open_button.place(x = 1670, y = 370+85) 737 | 738 | 739 | move_button = tk.Button(root, text = "Move", font = (letter_font,23), width = 15, height = 1,borderwidth=3,command = lambda: raise_frame(move_frame,move_button,log_button,teach_button,setup_button,'move')) 740 | move_button.place(x = 0, y = 2) 741 | 742 | log_button = tk.Button(root, text = "Log", font = (letter_font,23), width = 15, height = 1,borderwidth=3 ,command = lambda: raise_frame(log_frame,log_button,move_button,teach_button,setup_button,'log')) 743 | log_button.place(x = 285, y = 2) 744 | 745 | teach_button = tk.Button(root, text = "Teach", font = (letter_font,23), width = 15, height = 1,borderwidth=3 ,command = lambda: raise_frame(teach_frame,teach_button,log_button,move_button,setup_button,'teach')) 746 | teach_button.place(x = 570, y = 2) 747 | 748 | setup_button = tk.Button(root, text = "Setup", font = (letter_font,23), width = 15, height = 1,borderwidth=3 ,command = lambda: raise_frame(setup_frame,setup_button,log_button,teach_button,move_button,'setup')) 749 | setup_button.place(x = 855, y = 2) 750 | 751 | 752 | # Stuff that need constant updating and here we define it 753 | 754 | btns_progress = [] 755 | btn_nr_ = -1 756 | ticks_label = [] 757 | Deg_label = [] 758 | RAD_label = [] 759 | Temperature_label = [] 760 | Current_label = [] 761 | pos_labels = [] 762 | pos_labels2 = [] 763 | 764 | pos_text = ['X: ','Y: ','Z: ','phi: ','theta: ','psi: '] 765 | # Euler angles tell us how to get from out base frame that is static to one our end-effector is now. 766 | # We do it by rotating for 'phi' around Z then 'theta' around Y and then 'psi' around Z again. 767 | 768 | robot_names = ['Base: ', 'Shoulder: ', 'Elbow: ', 'Wrist 1: ', 'Wrist 2: ', 'Wrist 3: '] 769 | 770 | Data_ = "#####" 771 | 772 | # Raise move frame as default 773 | raise_frame(move_frame,move_button,setup_button,teach_button,log_button,'move') 774 | p12[1] = 0 775 | move_frame_stuff() 776 | teach_frame_stuff() 777 | log_frame_stuff() 778 | setup_frame_stuff() 779 | 780 | # Create scale that allows to set speed of joints 781 | # Scales return value from 1-100 782 | speed_scale = tk.Scale(move_frame, from_=1, to=100, orient = tk.HORIZONTAL,bg = "white",borderwidth=3,length = 300, font = (letter_font,11)) 783 | speed_scale.place(x = 1008, y = 33) 784 | speed_scale.set(spd_set) 785 | 786 | # Create scale that allows to set acceleration of joints 787 | acc_scale = tk.Scale(move_frame, from_=1, to=100, orient = tk.HORIZONTAL,bg = "white",borderwidth=3,length = 300, font = (letter_font,11)) 788 | acc_scale.place(x = 688, y = 33) 789 | acc_scale.set(acc_set) 790 | 791 | speed_scale_l = tk.Label(move_frame, text="Speed [%]" ,font = (letter_font,13),bg = "white") 792 | speed_scale_l.place(x = 1008, y = 8) 793 | 794 | acc_scale_l = tk.Label(move_frame, text="Acceleration [%]" ,font = (letter_font,13),bg = "white") 795 | acc_scale_l.place(x = 688, y = 8) 796 | 797 | for y in range(1,7): 798 | 799 | # Most of this stuff is labels for jog, these lables will be updating constantly 800 | btn_nr_ += 1 801 | ticks_label.append(tk.Label(move_frame, text="Encoder: " + Data_,font = (letter_font,12),bg = "white")) 802 | ticks_label[btn_nr_].place(x = 250, y = 152+btn_nr_*135) 803 | 804 | Deg_label.append(tk.Label(move_frame, text="Degree: " + Data_,font = (letter_font,12),bg = "white")) 805 | Deg_label[btn_nr_].place(x = 250, y = 130+btn_nr_*135) 806 | 807 | RAD_label.append(tk.Label(move_frame, text="Radians: " + Data_,font = (letter_font,12),bg = "white")) 808 | RAD_label[btn_nr_].place(x = 250, y = 108+btn_nr_*135) 809 | 810 | Temperature_label.append(tk.Label(move_frame, text="Temperature: " + Data_,font = (letter_font,12),bg = "white")) 811 | Temperature_label[btn_nr_].place(x = 425, y = 130+btn_nr_*135) 812 | 813 | Current_label.append(tk.Label(move_frame, text="Current: " + Data_,font = (letter_font,12),bg = "white")) 814 | Current_label[btn_nr_].place(x = 425, y = 108+btn_nr_*135) 815 | 816 | btns_progress.append(Progressbar(move_frame, orient = tk.HORIZONTAL, length = 350, mode = 'determinate')) 817 | btns_progress[btn_nr_].place(x = 250, y = 180+btn_nr_*135) 818 | 819 | pos_labels.append(tk.Label(root, text=pos_text[btn_nr_] + Data_,font = (letter_font,14),bg = "white")) 820 | pos_labels[btn_nr_].place(x = 1450, y = 45+btn_nr_*35+ 85) 821 | 822 | pos_labels2.append(tk.Label(root, text=robot_names[btn_nr_] + Data_,font = (letter_font,14),bg = "white")) 823 | pos_labels2[btn_nr_].place(x = 1585, y = 45+btn_nr_*35+ 85) 824 | 825 | 826 | 827 | 828 | #### Stuff that will need to be updated after some time e.g. progress bars, x,y,z values... ######### 829 | def Stuff_To_Update(): 830 | global spd_set, acc_set 831 | spd_set = speed_scale.get() 832 | acc_set = acc_scale.get() 833 | 834 | acc_scale_l.configure(text = "Acceleration " + str(acc_set) + "%") 835 | speed_scale_l.configure(text = "Speed " + str(spd_set) + "%") 836 | 837 | #T = robot_arm.fkine(p7) # Calculate get homogenous transformation matrix for current joint angles 838 | 839 | # Update motor pos for only joint_num available joints 840 | for y in range(0,rbt.Joint_num): 841 | btns_progress[y]["value"] = int(np.interp(p6[y],rbt.Joint_limits_ticks[y],[0,100])) 842 | btns_progress[y].update() 843 | ticks_label[y].configure(text="Encoder: " + str(p6[y])) 844 | Deg_label[y].configure(text="Degree: " + str(round(rbt.RAD2D(p7[y]) ,4)) + " °") 845 | RAD_label[y].configure(text="Radians: " + str(round(p7[y], 6))) #raw_var * 0.04394531 * (np.pi / 180, 3) 846 | Temperature_label[y].configure(text="Temperature: " + str(p9[y]) + " ℃") 847 | Current_label[y].configure(text="Current: " + str(round(p8[y]/1000, 5)) + " A") 848 | pos_labels2[y].configure(text= robot_names[y] +str(round(p7[y],4 ))) 849 | 850 | for y in range(0,6): 851 | pos_labels[y].configure(text= pos_text[y] +str(round(p10[y],4 ))) 852 | 853 | root.after(95,Stuff_To_Update) # update data every 25 ms 854 | 855 | root.after(1, Stuff_To_Update) 856 | root.mainloop() 857 | 858 | 859 | 860 | 861 | def do_stuff(left_btns,right_btns,raw_ENC,True_rads,spd_set,acc_set,current,temperature,True_pose,RPM_speed,True_rads_speed,grav_pos,software_control,Steer_K1,Steer_K2,Steer_K3,Steer_K4): 862 | 863 | gt.Clear_Error(1) 864 | gt.Clear_Error(2) 865 | gt.Clear_Error(3) 866 | gt.Clear_Error(4) 867 | gt.Clear_Error(5) 868 | gt.Clear_Error(6) 869 | time.sleep(0.01) 870 | gt.Change_data(1,200,3000,10000) 871 | gt.Change_data(2,200,5000,10000) 872 | gt.Change_data(3,200,3000,10000) 873 | gt.Change_data(4,200,3000,10000) 874 | gt.Change_data(5,200,3000,10000) 875 | gt.Change_data(6,200,3000,10000) 876 | time.sleep(0.01) 877 | 878 | 879 | # Array where freeform recorded positions are stored 880 | freeform_record_array = np.empty(shape=(15000,6),order='C',dtype='object') 881 | # variable that tells us how many steps we took in our freeform recording 882 | freeform_record_len = 0 883 | current_freefrom_step = 0 884 | current_freefrom_step_execute = 0 885 | 886 | 887 | matrix_array = np.empty(shape=(0,6),order='C',dtype='object') 888 | 889 | True_pose_var = [None] * 6 #Variable that stores pose of robot. Index in order: X,Y,Z,R,R,R 890 | 891 | hold_var = [0] * rbt.Joint_num # Hold var is 1 if robot is holding position 892 | Direction_var = [None] * rbt.Joint_num # None for not moving, True and False for directions 893 | 894 | 895 | # Stuff for accelerated jogging of motors 896 | current_speed = [0] * rbt.Joint_num 897 | acc_cntr = [0] * rbt.Joint_num 898 | 899 | Speed_jog_setpoint = [0] * rbt.Joint_num 900 | Acceleration_jog_setpoint = [0] * rbt.Joint_num 901 | ################################### 902 | 903 | prev_enable_disable = 0 904 | Sending_stuff = 0 # if it is 0 then send dummy data if it is 1 send nothing since we are sending usefull data 905 | prev_motor_mode_select = 1 906 | 907 | # code execution control variables 908 | started_execution = 0 # 0 if we are not executing script, when script is run first time it goes to 1 and stays until script stops executing 909 | clean_string = [] # whole code of a executing script cleaned up. That means that every '\n' is removed at 910 | code_step_cntr = 0 # tells us at what line of code we are i.e. what line of code we are executing atm 911 | time_step_cntr = 0 # Tells us at what time we are at specific line of code 912 | step_time = 0 # Tells us how long each line of code need to last 913 | number_of_code_lines = 0 # Number of code lines in script 914 | Security_flag = 0 # Tells us if security is triggered. 0 if not 1 if triggered 915 | current_current_limit = rbt.Default_current_limits 916 | security_stop_duration = rbt.Default_security_stop_duration 917 | security_counter = 0 918 | 919 | current_pos_command = [None]*rbt.Joint_num # if in pos mode. Save current commanded position here. It will be reexecuted when security is over 920 | current_speed_command = [None]*rbt.Joint_num # if in pos mode. Save current commanded speed here. It will be reexecuted when security is over 921 | current_command = '' # command that is being executed atm 922 | 923 | tt = 0 924 | while(1): 925 | try: 926 | bg = time.time() 927 | 928 | # Reads all data from serial port. This function also blocks 929 | ENC,RADS,cur,spd,spd_RAD,temp,vol,err= gt.main_comms_func() 930 | 931 | # This gets current robot pose and write to multi proc 932 | Enable_disable_var = software_control[0] # 0 is for disabled / 1 is for enabled 933 | operating_mode = software_control[1] 934 | 935 | T = robot_arm.fkine(RADS) 936 | T2 = T*1 937 | True_pose_var[0] = T2[0][3] 938 | True_pose_var[1] = T2[1][3] 939 | True_pose_var[2] = T2[2][3] 940 | True_pose_var[3:] = T.eul('deg') 941 | 942 | # This reads all multi process variables and writes to all multi process variables (With len of joint_num) 943 | for y in range(0,rbt.Joint_num): 944 | 945 | # Read multi proc 946 | # No need for this just read the index you want 947 | 948 | # Write multi proc 949 | raw_ENC[y] = ENC[y] 950 | True_rads[y] = RADS[y] 951 | current[y] = cur[y] 952 | temperature[y] = temp[y] 953 | RPM_speed[y] = spd[y] 954 | True_rads_speed[y] = spd_RAD[y] 955 | 956 | # This reads all multi process variables and writes to all multi process variables (With len of 6) 957 | for y in range(0,6): 958 | 959 | # Write multi proc 960 | True_pose[y] = True_pose_var[y] 961 | 962 | 963 | if np.any(cur > np.array(current_current_limit)) or Security_flag == 1: 964 | 965 | if security_counter == 0: 966 | Security_flag = 1 967 | 968 | gt.Strong_position_hold(1,7.5,1) 969 | gt.Strong_position_hold(2,7.5,1) 970 | gt.Strong_position_hold(3,7.5,1) 971 | 972 | if security_counter >= 0 and security_counter < ((security_stop_duration / rbt.Data_interval)): 973 | 974 | #print(security_counter) 975 | security_counter = security_counter + 1 976 | 977 | 978 | if security_counter == ((security_stop_duration / rbt.Data_interval)): 979 | 980 | security_counter = 0 981 | Security_flag = 0 982 | 983 | if current_command == 'pos': 984 | for y in range(0,rbt.Joint_num): 985 | gt.GOTO_position_HOLD(y+1,current_pos_command[y],current_speed_command[y],7.5,1) 986 | 987 | 988 | ### Send dummy data when nobody else is sending data 989 | if Sending_stuff == 0: 990 | gt.send_dummy_data() 991 | #print("dummy_data") 992 | else: 993 | #print("not_dummy_data") 994 | None 995 | ######################## 996 | 997 | # If in teach mode 998 | if (operating_mode == 2 or operating_mode == 0) and Enable_disable_var == 1 and Security_flag == 0: 999 | 1000 | if software_control[3] == 1: # stared exectuting script 1001 | 1002 | if started_execution == 0: 1003 | text_file = open(Image_path + "/Programs/execute_script.txt",'r') 1004 | code_string = text_file.readlines() 1005 | text_file.close() 1006 | 1007 | for i in range(0,len(code_string)): 1008 | if code_string[i] == '\n': 1009 | continue 1010 | else: 1011 | clean_string.append(code_string[i]) 1012 | 1013 | if clean_string[len(clean_string)-1] == 'end\n' or clean_string[len(clean_string)-1] == 'loop\n': 1014 | valid_data = 1 1015 | else: 1016 | valid_data = 0 1017 | 1018 | started_execution = 1 1019 | code_step_cntr = 0 1020 | time_step_cntr = 0 1021 | number_of_code_lines = len(clean_string) 1022 | step_time = 0 1023 | 1024 | 1025 | if code_step_cntr < number_of_code_lines: 1026 | Sending_stuff = 1 1027 | if time_step_cntr == 0: 1028 | code2execute = clean_string[code_step_cntr].split(',') 1029 | code2execute = code2execute[:-1] 1030 | #print(clean_string) 1031 | print(code2execute) 1032 | 1033 | if(code2execute[0] == 'pos'): 1034 | 1035 | step_time = float(code2execute[1]) # data after index 1 is position data and index 1 is time data 1036 | start_pos = [None]*rbt.Joint_num 1037 | stop_pos = [None]*rbt.Joint_num 1038 | 1039 | current_command = 'pos' 1040 | 1041 | for y in range(0,rbt.Joint_num): 1042 | 1043 | start_pos[y] = True_rads[y] 1044 | stop_pos[y] = float(code2execute[y+2]) 1045 | 1046 | pos_var,spd_var = rbt.GO_TO_POSE(start_pos, stop_pos,step_time) 1047 | 1048 | current_pos_command = pos_var 1049 | current_speed_command = spd_var 1050 | 1051 | for y in range(0,rbt.Joint_num): 1052 | gt.GOTO_position_HOLD(y+1,pos_var[y],spd_var[y],7.5,1) 1053 | 1054 | # send movement shit 1055 | 1056 | elif(code2execute[0] == 'CSAAR'): 1057 | 1058 | step_time = float(code2execute[1]) # data after index 1 is position data and index 1 is time data 1059 | start_pos = [None]*rbt.Joint_num 1060 | stop_pos = [None]*rbt.Joint_num 1061 | 1062 | current_command = 'CSAAR' 1063 | 1064 | for y in range(0,rbt.Joint_num): 1065 | 1066 | start_pos[y] = True_rads[y] 1067 | stop_pos[y] = float(code2execute[y+2]) 1068 | 1069 | 1070 | matrix_array = np.empty(shape=( int( step_time / rbt.Data_interval ),6),order='C',dtype='object') 1071 | matrix_array = rbt.CSAAR(start_pos,stop_pos,step_time) 1072 | for m in range(0,rbt.Joint_num): 1073 | gt.teleop_mode(m+1,rbt.RAD2E(matrix_array[time_step_cntr][m],m),0,Steer_K1[m],Steer_K2[m],Steer_K3[m],Steer_K4[m]) 1074 | 1075 | 1076 | elif(code2execute[0] == 'JTRAJ'): 1077 | 1078 | step_time = float(code2execute[1]) # data after index 1 is position data and index 1 is time data 1079 | start_pos = [None]*rbt.Joint_num 1080 | stop_pos = [None]*rbt.Joint_num 1081 | 1082 | current_command = 'JTRAJ' 1083 | 1084 | for y in range(0,rbt.Joint_num): 1085 | 1086 | start_pos[y] = True_rads[y] 1087 | stop_pos[y] = float(code2execute[y+2]) 1088 | 1089 | 1090 | matrix_array = np.empty(shape=( int( step_time / rbt.Data_interval ),6),order='C',dtype='object') 1091 | temp_var = rp.tools.trajectory.jtraj(start_pos,stop_pos,int( step_time / rbt.Data_interval )) 1092 | matrix_array = temp_var.q 1093 | for m in range(0,rbt.Joint_num): 1094 | gt.teleop_mode(m+1,rbt.RAD2E(matrix_array[time_step_cntr][m],m),0,Steer_K1[m],Steer_K2[m],Steer_K3[m],Steer_K4[m]) 1095 | 1096 | 1097 | elif(code2execute[0] == 'CTRAJ'): 1098 | 1099 | step_time = float(code2execute[1]) # data after index 1 is position data and index 1 is time data 1100 | start_pos = [None]*rbt.Joint_num 1101 | stop_pos = [None]*rbt.Joint_num 1102 | 1103 | current_command = 'CTRAJ' 1104 | 1105 | for y in range(0,rbt.Joint_num): 1106 | 1107 | start_pos[y] = True_rads[y] 1108 | stop_pos[y] = float(code2execute[y+2]) 1109 | 1110 | 1111 | matrix_array = np.empty(shape=( int( step_time / rbt.Data_interval ),6),order='C',dtype='object') 1112 | temp_var = rp.tools.trajectory.jtraj(start_pos,stop_pos,int( step_time / rbt.Data_interval )) 1113 | matrix_array = temp_var.q 1114 | for m in range(0,rbt.Joint_num): 1115 | gt.teleop_mode(m+1,rbt.RAD2E(matrix_array[time_step_cntr][m],m),0,Steer_K1[m],Steer_K2[m],Steer_K3[m],Steer_K4[m]) 1116 | 1117 | elif(code2execute[0] == 'delay'): 1118 | 1119 | current_command = 'delay' 1120 | step_time = float(code2execute[1]) 1121 | gt.send_dummy_data() 1122 | 1123 | elif(code2execute[0] == 'end'): 1124 | 1125 | current_command = 'end' 1126 | code_step_cntr = 0 1127 | step_time = 0 1128 | time_step_cntr = 0 1129 | started_execution = 0 1130 | software_control[3] = 0 1131 | clean_string = [] 1132 | 1133 | elif(code2execute[0] == 'loop'): 1134 | 1135 | current_command = 'loop' 1136 | code_step_cntr = 0 1137 | step_time = 0 1138 | time_step_cntr = 0 1139 | software_control[3] = 1 1140 | started_execution = 1 1141 | 1142 | 1143 | elif time_step_cntr > 0 and time_step_cntr < ((step_time / rbt.Data_interval)): 1144 | if(current_command == 'CSAAR'): 1145 | #print(rbt.RAD2E((matrix_array[time_step_cntr][5]),5)) 1146 | #gt.teleop_mode(6,rbt.RAD2E(matrix_array[time_step_cntr][5],5),0,Steer_K1[5],Steer_K2[5],Steer_K3[5],Steer_K4[5]) 1147 | #print(matrix_array) 1148 | for m in range(0,rbt.Joint_num): 1149 | gt.teleop_mode(m+1,rbt.RAD2E(matrix_array[time_step_cntr][m],m),0,Steer_K1[m],Steer_K2[m],Steer_K3[m],Steer_K4[m]) 1150 | 1151 | elif(current_command == 'JTRAJ'): 1152 | for m in range(0,rbt.Joint_num): 1153 | gt.teleop_mode(m+1,rbt.RAD2E(matrix_array[time_step_cntr][m],m),0,Steer_K1[m],Steer_K2[m],Steer_K3[m],Steer_K4[m]) 1154 | 1155 | else: 1156 | gt.send_dummy_data() 1157 | #print("dummy data" + str(time_step_cntr)) 1158 | 1159 | if time_step_cntr < ((step_time / rbt.Data_interval) ): 1160 | 1161 | time_step_cntr = time_step_cntr + 1 1162 | 1163 | #print(time_step_cntr) 1164 | 1165 | if time_step_cntr == ((step_time / rbt.Data_interval)) and current_command != 'loop': 1166 | time_step_cntr = 0 1167 | step_time = 0 1168 | code_step_cntr = code_step_cntr + 1 1169 | 1170 | 1171 | elif software_control[3] == 0: # stop executing. Stops script completely, execute will rerun the script 1172 | 1173 | Sending_stuff = 0 1174 | code_step_cntr = 0 1175 | step_time = 0 1176 | time_step_cntr = 0 1177 | started_execution = 0 1178 | clean_string = [] 1179 | 1180 | elif software_control[3] == 2: # pause executing, meaning that after you press pause press execute it will continue where it left off 1181 | Sending_stuff = 0 1182 | None 1183 | 1184 | # This stuff is for enabling and disabling(gravity comp or disable) specific motors on left side panel 1185 | if prev_motor_mode_select != software_control[2]: 1186 | prev_motor_mode_select = software_control[2] 1187 | for y in range(0, rbt.Joint_num): 1188 | if grav_pos[y] == 1: 1189 | gt.Strong_position_hold(y+1,7.5,1) 1190 | if grav_pos[y] == 0 and grav_pos[6] == 1: 1191 | gt.Gravity_compensation(y+1,20,5) 1192 | if grav_pos[y] == 0 and grav_pos[6] == 0: 1193 | gt.Disable(y+1) 1194 | 1195 | # Jog motors WITH acceleration 1196 | 1197 | 1198 | 1199 | if operating_mode == 0 and Enable_disable_var == 1 and Security_flag == 0: 1200 | 1201 | for y in range(0,rbt.Joint_num): 1202 | 1203 | # Reads positions from sliders or slow,default,fast setting and scales for each joint 1204 | Speed_jog_setpoint[y] = np.interp(spd_set.value,[1,100],[rbt.Joint_min_speed[y],rbt.Joint_max_speed[y]]) 1205 | Acceleration_jog_setpoint[y] = np.interp(acc_set.value,[1,100],[rbt.Joint_min_acc[y],rbt.Joint_max_acc[y]]) 1206 | 1207 | # Acceleration in negative direction of robots joint 1208 | # NOTE: directions follow right hand rule: 1209 | # * your tumb on right hand is positive direction of z axes, and fingers represent positive rotation. 1210 | # * Axes are defined by DH params 1211 | if left_btns[y] == 1 and right_btns[y] == 0: 1212 | Sending_stuff = 1 1213 | #print("jog") 1214 | current_speed[y] = rbt.Joint_min_speed[y] + acc_cntr[y] * rbt.Data_interval * Acceleration_jog_setpoint[y] 1215 | if(current_speed[y] >= Speed_jog_setpoint[y]): 1216 | current_speed[y] = Speed_jog_setpoint[y] 1217 | 1218 | gt.Speed_Dir(y+1, 0 if rbt.Direction_offsets[y] == 1 else 1,rbt.RADS2RPM(current_speed[y],y)) 1219 | acc_cntr[y] = acc_cntr[y] + 1 1220 | Direction_var[y] = True 1221 | hold_var[y] = 0 1222 | 1223 | 1224 | # Acceleration in positive direction of robots joint 1225 | if right_btns[y] == 1 and left_btns[y] == 0: 1226 | Sending_stuff = 1 1227 | #print("jog") 1228 | current_speed[y] = rbt.Joint_min_speed[y] + acc_cntr[y] * rbt.Data_interval * Acceleration_jog_setpoint[y] 1229 | if(current_speed[y] >= Speed_jog_setpoint[y]): 1230 | current_speed[y] = Speed_jog_setpoint[y] 1231 | 1232 | gt.Speed_Dir(y+1, 1 if rbt.Direction_offsets[y] == 1 else 0,rbt.RADS2RPM(current_speed[y],y)) 1233 | acc_cntr[y] = acc_cntr[y] + 1 1234 | Direction_var[y] = False 1235 | hold_var[y] = 0 1236 | 1237 | # Deacceleration 1238 | if current_speed[y] >= rbt.Joint_min_speed[y] and left_btns[y] == 0 and right_btns[y] == 0 and hold_var[y] == 0 and Direction_var[y] != None: 1239 | Sending_stuff = 1 1240 | #print("jog") 1241 | current_speed[y] = current_speed[y] - rbt.Data_interval * Acceleration_jog_setpoint[y] 1242 | 1243 | if(current_speed[y] <= rbt.Joint_min_speed[y]): 1244 | current_speed[y] = rbt.Joint_min_speed[y] 1245 | Direction_var[y] = None 1246 | 1247 | if Direction_var[y] == False: 1248 | gt.Speed_Dir(y+1, 1 if rbt.Direction_offsets[y] == 1 else 0,rbt.RADS2RPM(current_speed[y],y)) 1249 | elif Direction_var[y] == True: 1250 | gt.Speed_Dir(y+1, 0 if rbt.Direction_offsets[y] == 1 else 1,rbt.RADS2RPM(current_speed[y],y)) 1251 | 1252 | acc_cntr[y] = 0 1253 | 1254 | # If no button is pressed and we stopped deaccelerating, hold position 1255 | if left_btns[y] == 0 and right_btns[y] == 0 and hold_var[y] == 0 and Direction_var[y] == None: 1256 | gt.Strong_position_hold(y+1, 7.5, 1) # OVO SU JAKO DOBRE VRIJEDNOSTI (y+1, 7.5, 1) SA 17000 UPDATE RATE samo kp 10 je ok bez struje 1257 | Sending_stuff = 0 1258 | #print("jog") 1259 | #gt.Gravity_compensation(y+1,50,5) 1260 | acc_cntr[y] = 0 1261 | hold_var[y] = 1 1262 | 1263 | # When we disable robot 1264 | if Enable_disable_var == 0 and prev_enable_disable == 1: 1265 | prev_enable_disable = Enable_disable_var 1266 | 1267 | gt.Gravity_compensation(1,50,3) 1268 | gt.Gravity_compensation(2,50,3) 1269 | gt.Gravity_compensation(3,50,3) 1270 | gt.Gravity_compensation(4,50,3) 1271 | gt.Gravity_compensation(5,50,3) 1272 | gt.Gravity_compensation(6,50,3) 1273 | 1274 | #elif grav_pos[6] == 0: 1275 | #gt.Disable(1) 1276 | #gt.Disable(2) 1277 | #gt.Disable(3) 1278 | 1279 | # When we enable robot 1280 | elif Enable_disable_var == 1 and prev_enable_disable == 0: 1281 | prev_enable_disable = Enable_disable_var 1282 | for y in range(0, rbt.Joint_num): 1283 | gt.Enable(y+1) 1284 | gt.teleop_mode(y+1,raw_ENC[y],0,Steer_K1[y],Steer_K2[y],Steer_K3[y],Steer_K4[y]) 1285 | 1286 | 1287 | #print(time.time() - bg) 1288 | 1289 | # If freeform recordiong is on 1290 | if(software_control[4] == 1 and software_control[5] == 0): 1291 | freeform_record_array[current_freefrom_step][:] = True_rads 1292 | #print(freeform_record_array[current_freefrom_step][:]) 1293 | current_freefrom_step = current_freefrom_step + 1 1294 | freeform_record_len = current_freefrom_step 1295 | # If executing freeform movement 1296 | 1297 | if(software_control[4] == 0 and software_control[5] == 1): 1298 | 1299 | if(current_freefrom_step_execute == 0): 1300 | for y in range(0,rbt.Joint_num): 1301 | gt.GOTO_position_HOLD(y+1,rbt.RAD2E(freeform_record_array[0][y],y),30,5.5,1) 1302 | #print("firstGOTO i sleep 3s") 1303 | time.sleep(3) 1304 | 1305 | for m in range(0,rbt.Joint_num): 1306 | gt.teleop_mode(m+1,rbt.RAD2E(freeform_record_array[current_freefrom_step_execute][m],m),0,Steer_K1[m],Steer_K2[m],Steer_K3[m],Steer_K4[m]) 1307 | 1308 | #print(current_freefrom_step_execute) 1309 | if(current_freefrom_step_execute == freeform_record_len - 1): 1310 | current_freefrom_step_execute = 0 1311 | software_control[5] = 0 1312 | #print("done") 1313 | 1314 | if(software_control[5] == 1): 1315 | current_freefrom_step_execute = current_freefrom_step_execute + 1 1316 | 1317 | # Ako je current_freefrom_step_execute == freeform_record_len 1318 | # software_control[5] = 0 1319 | # current_freefrom_step_execute = 0 1320 | 1321 | 1322 | 1323 | 1324 | # If we want to show plot 1325 | if(software_control[6] == 1): 1326 | #print(freeform_record_len) 1327 | software_control[6] = 0 1328 | plt.plot(freeform_record_array) 1329 | plt.show() 1330 | 1331 | # Clear all recorded 1332 | if(software_control[7] == 1): 1333 | software_control[7] = 0 1334 | freeform_record_array = np.empty(shape=(15000,6),order='C',dtype='object') 1335 | # variable that tells us how many steps we took in our freeform recording 1336 | freeform_record_len = 0 1337 | current_freefrom_step = 0 1338 | tt = tt + 1 1339 | if(tt == 10): 1340 | print((time.time() - bg)) 1341 | tt = 0 1342 | #################################### 1343 | except: 1344 | gt.try_reconnect() 1345 | 1346 | 1347 | def show_graph(p1, p2, p3, p4, p5, p6): 1348 | 1349 | 1350 | while(1): 1351 | #print(p5.value) 1352 | #print(p6.value) 1353 | if p6.value == 0: 1354 | plots.runGraph(p1,p2,p3,p4,p5.value,p6.value) 1355 | p6.value = 1 1356 | 1357 | 1358 | if __name__ == "__main__": 1359 | 1360 | Setpoint_Speed_Proc = multiprocessing.Value('d',0) # return value of speed setpoint 1361 | Setpoint_Acc_Proc = multiprocessing.Value('d',0) # return value of acceleration setpoint 1362 | 1363 | # return value of what translation button is pressed 1364 | # Variables go like this X+,X-,Y+,Y-,Z+,Z- 1365 | Translations_Proc = multiprocessing.Array("i",6, lock=False) 1366 | 1367 | # Variables go from top top bottom and 1 represents pressed and 0 released 1368 | Left_btns_Proc = multiprocessing.Array("i",6, lock=False) # return value of what left button is pressed 1369 | Right_btns_Proc = multiprocessing.Array("i",6, lock=False) # return value of what right button is pressed 1370 | 1371 | software_control_variables = multiprocessing.Array("i",8, lock=False) # variables are index values: 1372 | #Index 0: Enable/Disable robot. Disable places it in position hold mode, Enable allows to use other functions(jog, teach...) 1373 | # 0 is for disabled / 1 is for enabled 1374 | #Index 1: What window is open: 0 - Move, 1 - log, 2 - Teach, 3 - setup 1375 | #Index 2: variable to set motor modes in teach mode/position hold 1376 | #Index 3: Execute flag. If 1 we are executing script if 0 we are not 1377 | #Index 4: Recording freeform movement 1 is for recording 0 for not recordning 1378 | #Index 5: 1 is for executing freeform movement 0 is for not executing freeform 1379 | #Index 6: Show plot 1 is to trigger show 1380 | #Index 7: Clear all recorded 1381 | 1382 | 1383 | grav_pos_flag = multiprocessing.Array("i",[1,1,1,1,1,1,1], lock=False) # Used to log what joints should be in gravity compensation and what should be in position hold. 1384 | # Index 6 (7nth variable) is used to tell us if we are in gravity comp(1) or disable motor(0) 1385 | 1386 | # These are variables we get packed in one string from master serial port 1387 | # Len is the number of joints available 1388 | p_position = multiprocessing.Array("i", rbt.Joint_num, lock=False) # Raw encoder ticks 1389 | p_position_RADS = multiprocessing.Array("d", rbt.Joint_num, lock=False) # True radians position for kinematic model 1390 | # this includes all offsets and conversions so the robot matches his kinematic model 1391 | p_speed = multiprocessing.Array("i", rbt.Joint_num, lock=False) # Raw Speed in RPM 1392 | p_speed_RADS = multiprocessing.Array("d", rbt.Joint_num, lock=False) # True speed in RAD/S 1393 | p_current = multiprocessing.Array("i", rbt.Joint_num, lock=False) 1394 | p_temperature = multiprocessing.Array("i", rbt.Joint_num, lock=False) 1395 | p_voltage = multiprocessing.Value('i',0) 1396 | p_error = multiprocessing.Value('i',0) 1397 | 1398 | proc_value_show_plot = multiprocessing.Value('i',2) 1399 | proc_value_close_plot = multiprocessing.Value('i',0) 1400 | 1401 | p_robot_pose = multiprocessing.Array("d", 6, lock=False) # Current pose of the robot 1402 | 1403 | # Variables for Steer mode 1404 | Steer_K1 = multiprocessing.Array("d", rbt.Joint_num, lock=False) 1405 | Steer_K2 = multiprocessing.Array("d", rbt.Joint_num, lock=False) 1406 | Steer_K3 = multiprocessing.Array("d", rbt.Joint_num, lock=False) 1407 | Steer_K4 = multiprocessing.Array("d", rbt.Joint_num, lock=False) 1408 | 1409 | 1410 | process1 = multiprocessing.Process(target=Tkinter_GUI,args=[Setpoint_Speed_Proc,Setpoint_Acc_Proc,Translations_Proc,Left_btns_Proc, 1411 | Right_btns_Proc,p_position,p_position_RADS,p_current,p_temperature, 1412 | p_robot_pose,grav_pos_flag,software_control_variables, 1413 | Steer_K1,Steer_K2,Steer_K3,Steer_K4]) 1414 | 1415 | process2 = multiprocessing.Process(target=do_stuff,args=[Left_btns_Proc,Right_btns_Proc,p_position,p_position_RADS,Setpoint_Speed_Proc, 1416 | Setpoint_Acc_Proc,p_current,p_temperature,p_robot_pose,p_speed,p_speed_RADS, 1417 | grav_pos_flag,software_control_variables,Steer_K1,Steer_K2,Steer_K3,Steer_K4]) 1418 | 1419 | 1420 | #proc_position, proc_speed, proc_current, proc_temperature, proc_plot_show, close_event 1421 | process3 = multiprocessing.Process(target=show_graph,args=[p_position_RADS,p_speed_RADS,p_current,p_temperature,proc_value_show_plot,proc_value_close_plot]) 1422 | 1423 | process1.start() 1424 | process2.start() 1425 | process3.start() 1426 | process1.join() 1427 | process2.join() 1428 | process3.join() 1429 | process1.terminate() 1430 | process2.terminate() 1431 | process3.terminate() -------------------------------------------------------------------------------- /Robot_Program/Programs/CSR_2.txt: -------------------------------------------------------------------------------- 1 | CSR,1.6,0.0092,1.05173,-0.606,0.01416,-1.15253,-0.08836, 2 | delay,1, 3 | CSR,1.6,-0.17717,1.72054,0.552,-0.27187,0.77722,-0.14481, 4 | delay,1, 5 | CSR,1.6,0.7278,1.92533,-0.7362,-0.6013,-0.46224,-0.72649, 6 | delay,1, 7 | CSR,4,1.185,0.69584,-0.01537,-0.21995,0.47758,-0.99402, 8 | delay,1, 9 | CSR,2,-0.3572,1.393,-0.71669,-0.354,-1.03084,-1.22228, 10 | delay,1, 11 | loop, 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /Robot_Program/Programs/CSR_BIG_SPEED.txt: -------------------------------------------------------------------------------- 1 | CSR,2,1.6521,0.59,-0.61468,0.17181,-0.17487,-0.32643, 2 | delay,1, 3 | CSR,2,1.10063,0.56545,-0.39187,0.10101,0.18101,-0.32643, 4 | delay,1, 5 | CSR,2,1.51864,0.52941,-0.1704,0.30396,0.21271,-0.32643, 6 | delay,1, 7 | CSR,2,1.72803,0.57159,-0.32677,0.40119,0.28123,-0.32643, 8 | delay,1, 9 | CSR,2,1.08069,0.34073,0.3618,0.64474,0.61155,-0.32643, 10 | delay,1, 11 | loop, 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /Robot_Program/Programs/CSSAR_TEST_DOBAR.txt: -------------------------------------------------------------------------------- 1 | CSR,2,1.5892,0.65979,-0.26905,0.47766,-0.02045,-0.30802, 2 | delay,2, 3 | CSR,2,1.13898,0.53401,0.14771,0.04059,0.35384,-0.30802, 4 | delay,2, 5 | CSR,2,1.86455,0.62912,-0.39992,0.62209,-1.46751,-0.3082, 6 | delay,2, 7 | CSR,2,0.64581,0.70505,-0.71266,-0.75141,-1.69454,-0.3082, 8 | delay,2, 9 | loop, 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /Robot_Program/Programs/GRIPPER_TEST.txt: -------------------------------------------------------------------------------- 1 | CSR,2,1.5892,0.65979,-0.26905,0.47766,-0.02045,-0.30802, 2 | delay,2, 3 | CSR,2,1.13898,0.53401,0.14771,0.04059,0.35384,-0.30802, 4 | delay,2, 5 | CSR,2,1.86455,0.62912,-0.39992,0.62209,-1.46751,-0.3082, 6 | delay,2, 7 | CSR,2,0.64581,0.70505,-0.71266,-0.75141,-1.69454,-0.3082, 8 | delay,2, 9 | loop, 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /Robot_Program/Programs/MOVE_123.txt: -------------------------------------------------------------------------------- 1 | CSR,1,1.19267,1.82485,-0.38717,1.49056,-0.6095,-0.28103, 2 | delay,1, 3 | CSR,1,-1.03697,1.28873,0.224,-1.83983,-1.16173,-0.28225, 4 | delay,1, 5 | CSR,1,-0.655,0.74493,-0.51535,-0.39176,-1.32229,-0.28225, 6 | delay,1, 7 | CSR,2.5,1.16122,0.74676,0.29334,2.75267,-0.99606,-0.285, 8 | delay,1, 9 | CSR,1,-0.05752,1.95447,0.38059,0.40119,1.63829,-0.2825, 10 | delay,1, 11 | CSR,1,-0.77236,0.9014,2.6087,-2.33543,-1.3949,-0.28225, 12 | delay,1, 13 | CSR,1,-0.09741,1.41375,-0.67038,0.09062,-0.70768,-0.285, 14 | delay,1, 15 | loop, 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /Robot_Program/Programs/NEDIRAJ.txt: -------------------------------------------------------------------------------- 1 | CSR,1.6,0.0092,1.05173,-0.606,0.01416,-1.15253,-0.08836, 2 | delay,1, 3 | CSR,1.6,-0.17717,1.72054,0.552,-0.27187,0.77722,-0.14481, 4 | delay,1, 5 | CSR,1.6,0.7278,1.92533,-0.7362,-0.6013,-0.46224,-0.72649, 6 | delay,1, 7 | CSR,3,1.185,0.69584,-0.01537,-0.21995,0.47758,-0.99402, 8 | delay,2, 9 | CSR,1.4,-0.3572,1.393,-0.71669,-0.354,-1.03084,-1.22228, 10 | delay,1, 11 | loop, 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /Robot_Program/Programs/execute_script.txt: -------------------------------------------------------------------------------- 1 | JTRAJ,2,0.46786,1.55564,-0.55696,0.26054,-0.55019,-0.95475, 2 | delay,1, 3 | JTRAJ,2,1.50407,0.53017,-0.48515,0.09157,-1.54932,-0.95475, 4 | delay,1, 5 | JTRAJ,2,1.50867,0.49106,-0.09926,0.1416,-1.2732,-0.95475, 6 | delay,1, 7 | JTRAJ,2,1.44424,0.61147,-0.00732,0.1416,-1.11878,-0.95475, 8 | delay,1, 9 | JTRAJ,2,1.25173,1.13303,0.99802,0.32284,0.29861,-0.95475, 10 | delay,1, 11 | JTRAJ,2,0.11198,1.71211,-0.1563,0.12744,0.16772,-0.95475, 12 | delay,1, 13 | loop, 14 | 15 | -------------------------------------------------------------------------------- /Robot_Program/Programs/test.txt: -------------------------------------------------------------------------------- 1 | pos,4,0.09664,0.64657,-0.00307, 2 | delay,1.5, 3 | pos,4,0.20479,1.69658,-1.47109, 4 | delay,1.5, 5 | end, 6 | 7 | -------------------------------------------------------------------------------- /Robot_Program/Programs/test1.txt: -------------------------------------------------------------------------------- 1 | sad 2 | sdadas 3 | saddas 4 | -------------------------------------------------------------------------------- /Robot_Program/Programs/test123.txt: -------------------------------------------------------------------------------- 1 | delay,0.2, 2 | pos,4,0.03068,0.87897,-1.49486, 3 | delay,1.5, 4 | pos,4,1.17656,0.54993,-1.49103, 5 | delay,1.5, 6 | pos,4,0.72941,0.68876,-1.4895, 7 | delay,1.5, 8 | pos,4,1.1758,0.53689,-1.4895, 9 | delay,1.5, 10 | loop, 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /Robot_Program/REAL_JTRAJ_SIM.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | @author Jesse Haviland 4 | """ 5 | 6 | import roboticstoolbox as rp 7 | from spatialmath import * 8 | import matplotlib 9 | import matplotlib.pyplot as plt 10 | import numpy as np 11 | import time 12 | import axes_robot as rbt 13 | 14 | fig, ax = plt.subplots(1, 4) 15 | 16 | # 3link robot 17 | link3 = rp.models.DH.three_link() 18 | 19 | 20 | # Joint start angles in radians 21 | q0 = [0,np.pi/2,0] 22 | 23 | # Joint stop angles in radians 24 | qf = [1.30235,0.129621,1.433505] 25 | 26 | s1 = time.time() 27 | 28 | # how long movement will take and what are time steps. In example: 29 | # (np.arange(0,10.5,0.01)) from 0 sec to 10.5s with 0.01 time step 30 | t2 = (np.arange(0,4,0.01)) 31 | print(len(t2)) 32 | 33 | start_speed = np.array([0.09,0.09,0.09]) 34 | stop_speed = np.array([0.09,0.09,0.09]) 35 | 36 | # Create trajectory 37 | qt = rp.tools.trajectory.jtraj(q0, qf, t2,start_speed,stop_speed) 38 | #tg = rp.tools.trajectory.ctraj(SE3.Rand(), SE3.Rand(), 20) 39 | 40 | s2 = time.time() 41 | 42 | print(s2-s1) 43 | #fig.suptitle('Position, speed and acceleration plots') 44 | 45 | ax[0].set_ylabel('Position [DEG]') 46 | ax[1].set_ylabel('Speed in [RPM]') 47 | ax[2].set_ylabel('Acceleration [RAD/S**2') 48 | 49 | #print(qt.q[:,0]) 50 | #print(rbt.RAD2E(qt.q[:,0],0)) 51 | 52 | print(abs(qt.qd[:,1])) 53 | print("penis") 54 | print(rbt.RADS2RPM(qt.qd[:,0],0)) 55 | #print(type(qt.q)) 56 | #print(len(qt.q)) 57 | #ax1.plot(qt.q[:,2]) 58 | #ax1.plot(qt.q[:,1]) 59 | #ax[0].plot(qt.q * (180/np.pi),linewidth=1.5) #DEG = RAD * (180 / np.pi) 60 | #ax[1].plot(qt.qd * (60/(2*np.pi)),linewidth=1.5) # RPM = rad/s * (60/(2*np.pi)) 61 | ax[0].plot(qt.q ,linewidth=1.5) #DEG = RAD * (180 / np.pi) 62 | ax[1].plot(qt.qd ,linewidth=1.5) # RPM = rad/s * (60/(2*np.pi)) 63 | ax[2].plot(qt.qdd,linewidth=1.5) 64 | 65 | plt.legend(["joint1","joint2","joint3"],bbox_to_anchor=(0., 1.02, 1., .102), loc='lower left', ncol=1, borderaxespad=0.) 66 | plt.show() 67 | 68 | link3.plot(qt.q.T, dt=10 ) 69 | 70 | link3.q = link3.qs 71 | 72 | print(link3.qs) 73 | T = link3.fkine(link3.qs) 74 | print(T) 75 | T2 = T*1 76 | print(T2[2,3] + 100) 77 | #R2 = SO3.Rz(30, 'deg') 78 | print(T.rpy('deg','zyx')) 79 | #print(T.angvec()) 80 | #print(R2) 81 | #print((T[0,1])) 82 | # Init joint to the 'ready' joint angles 83 | #panda.q = panda.qz 84 | 85 | # Open a plot with the teach panel 86 | # e = link3.teach() -------------------------------------------------------------------------------- /Robot_Program/Software_tests/CSAAR_test.py: -------------------------------------------------------------------------------- 1 | import roboticstoolbox as rtb 2 | from spatialmath import * 3 | import matplotlib 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import time 7 | 8 | # Define the robot 9 | robot = rtb.models.DH.CM6() 10 | print(robot) 11 | 12 | # 2 positions we will be testing 13 | # These are arrays filled with joint angles in radians 14 | q1 = np.array([0,np.pi/2,0,0.01,0.50,0]) 15 | q2 = np.array([0.31,0.5,2.0,0.70,0.3,0.70]) 16 | 17 | matrix = np.empty(shape=(0,6),order='C',dtype='object') 18 | print(matrix) 19 | print(type(matrix)) 20 | print(len(matrix)) 21 | 22 | def CSAAR(pose1,pose2,exec_time): 23 | Number_of_Steps = exec_time / 0.01 24 | Increment_steps = np.empty(shape=6,order='C',dtype='object') 25 | for n in range(0,6): 26 | Increment_steps[n] = (pose2[n] - pose1[n]) / Number_of_Steps 27 | 28 | Out_traj = np.empty(shape=(int(Number_of_Steps),6),order='C',dtype='object') 29 | 30 | for n in range(0, int(Number_of_Steps)): 31 | for m in range(0 ,6): 32 | Out_traj[n][m] = pose1[m] + Increment_steps[m] * (n + 1) 33 | 34 | return Out_traj 35 | 36 | bg = time.time() 37 | matrix = CSAAR(q1,q2,3) 38 | print(time.time() - bg) 39 | 40 | plt.plot(matrix) 41 | plt.show() 42 | 43 | print(matrix) 44 | print(type(matrix)) 45 | print(len(matrix)) 46 | 47 | if __name__ == "__main__": 48 | None 49 | 50 | 51 | -------------------------------------------------------------------------------- /Robot_Program/Software_tests/JTRAJ_test.py: -------------------------------------------------------------------------------- 1 | import roboticstoolbox as rtb 2 | from spatialmath import * 3 | import matplotlib 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import time 7 | 8 | # Define the robot 9 | robot = rtb.models.DH.CM6() 10 | print(robot) 11 | 12 | # 2 positions we will be testing 13 | # These are arrays filled with joint angles in radians 14 | q1 = np.array([0,np.pi/2,0,0.01,0.50,0]) 15 | q2 = np.array([0.31,0.5,2.0,0.70,0.3,0.70]) 16 | 17 | qx2 = rtb.tools.trajectory.jtraj(q1,q2,200) 18 | #print(qx2) 19 | #print(qx2.q) # Pozicije 20 | #print(qx2.qd) # Trebale bi biti brzine ali nisu ??? 21 | print(qx2.q) 22 | print(robot.fkine(qx2.q[199])) 23 | 24 | plt.plot(qx2.q) 25 | plt.show() -------------------------------------------------------------------------------- /Robot_Program/Software_tests/ctraj_test.py: -------------------------------------------------------------------------------- 1 | import roboticstoolbox as rtb 2 | #from spatialmath import * 3 | import matplotlib 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import time 7 | 8 | # Define the robot 9 | robot = rtb.models.DH.CM6() 10 | print(robot) 11 | 12 | # 2 positions we will be testing 13 | # These are arrays filled with joint angles in radians 14 | q1 = np.array([0,np.pi/2,0,0.01,0.50,0]) 15 | q2 = np.array([0.31,0.5,2.0,0.70,0.3,0.70]) 16 | 17 | # Time to execute movement 18 | # 50 time steps and we are using time_step of 10ms 19 | execution_time = 50 20 | print(execution_time) 21 | 22 | # Transform joint angles to homogenous transformation matrix 23 | T1 = (robot.fkine(q1)) 24 | T2 = (robot.fkine(q2)) 25 | print(T2) 26 | 27 | # Create Cartesian trajectory between two poses 28 | # Output of this function are Homogenous transformation matrices x execution time 29 | bg = time.time() 30 | TT2 = rtb.tools.trajectory.ctraj(T1, T2, execution_time) 31 | print(time.time() - bg) 32 | 33 | 34 | # Here we transform those Homogenous transformation matrices to joint angles 35 | # ikine_LM function returns bunch of stuff 36 | bg = time.time() 37 | qx1 = robot.ikine_LM(TT2) 38 | print(time.time() - bg) 39 | 40 | # Extract Joint angles from qx1 41 | qx1_q = [0] * execution_time 42 | for x in range(0,execution_time): 43 | qx1_q[x] = qx1[x].q 44 | #print(qx1_q[x]) 45 | 46 | # Prove that Commanded pose and true pose we got are the same! 47 | print(robot.fkine(qx1_q[execution_time-1])) 48 | 49 | # Plot our trajectory 50 | plt.plot(qx1_q) 51 | plt.show() 52 | 53 | -------------------------------------------------------------------------------- /Robot_Program/Software_tests/trajectory_tests.py: -------------------------------------------------------------------------------- 1 | import roboticstoolbox as rtb 2 | from spatialmath import * 3 | import matplotlib 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import time 7 | 8 | # Inicijalizacija plotova 9 | fig, ax = plt.subplots(1, 3) 10 | 11 | # ovak se radi execution time array 12 | # Vrijeme trajanja putanje može biti ovako ili preko integerea 13 | execution_time = (np.arange(0,4,0.01)) 14 | 15 | # Definiraj robot 16 | robot = rtb.models.DH.CM6() 17 | #print(robot) 18 | 19 | # Fkine of motor angles i dobijem jednu HG matricu T1 20 | T1 = robot.fkine(robot.qz) 21 | 22 | # Definiraj početni i zadnji joint pos i naporavi HG matrice od toga 23 | q1 = np.array([0,np.pi/2,0,0.01,0.50,0]) 24 | q2 = np.array([0.31,0.5,2.0,0.70,0.3,0.70]) 25 | 26 | T1 = (robot.fkine(q1)) 27 | print(T1) 28 | T2 = (robot.fkine(q2)) 29 | print(T2) 30 | 31 | len_time = 500 32 | # Ctraj radi, može se vrijeme pomoću array ili int 33 | TT2 = rtb.tools.trajectory.ctraj(T1, T2, len_time) 34 | #print(TT2) 35 | 36 | #Metode za izvršavanje inverzne kinematike 37 | qx1 = robot.ikine_LM(TT2) # radi dobro 38 | #print(qx1) 39 | #qx1 = robot.ikine_LMS(TT2) # radi 40 | #qx1 = robot.ikine_min(TT2) # radi dobro 41 | # qx1 je array sa dosta podataka a u qx1.q se nalaze joint pozicije 42 | 43 | 44 | print(type(qx1[0].q)) 45 | print(qx1[0].q) # 46 | 47 | # ovdje se moraju izvrtiti te joint pozicije i spremiti u novu var jer ga 48 | # zeza ovaj oblik koji dobijem ok ikine_LMS i drugih inverze kinematic metoda 49 | qx1_q = [0] * len_time 50 | for x in range(len_time): 51 | qx1_q[x] = qx1[x].q 52 | 53 | print(qx1_q[99]) 54 | print(robot.fkine(qx1_q[99])) 55 | 56 | 57 | # Jtraj radi 58 | qx2 = rtb.tools.trajectory.jtraj(q1,q2,200) 59 | #print(qx2) 60 | #print(qx2.q) # Pozicije 61 | #print(qx2.qd) # Trebale bi biti brzine ali nisu ??? 62 | print(qx2.q[199]) 63 | print(robot.fkine(qx2.q[199])) 64 | 65 | 66 | # ovo radi i isto je ko jtraj ako stavim tpoly 67 | qx3 = rtb.tools.trajectory.mtraj(rtb.tools.trajectory.tpoly,q1,q2,20) # TPOLY OR LSPB 68 | #print(qx3.q) 69 | 70 | ax[0].set_ylabel('ctraj') 71 | ax[1].set_ylabel('jtraj') 72 | ax[2].set_ylabel('mtraj') 73 | 74 | ax[0].plot(qx1_q ,linewidth=1.5) 75 | ax[1].plot(qx2.q ,linewidth=1.5) 76 | ax[2].plot(qx3.q ,linewidth=1.5) 77 | 78 | plt.legend(["joint1","joint2","joint3","joint4","joint5","joint6"],bbox_to_anchor=(0., 1.02, 1., .102), loc='lower left', ncol=2, borderaxespad=0.) 79 | plt.show() 80 | 81 | 82 | #robot.plot(qx1_q[99]) #plot poziciju konačnu 83 | 84 | robot.plot(robot.qs) 85 | # Mstraj test!! 86 | #qx2 = rtb.tools.trajectory.mstraj() 87 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /Robot_Program/__pycache__/axes_robot.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/__pycache__/axes_robot.cpython-36.pyc -------------------------------------------------------------------------------- /Robot_Program/__pycache__/get_send_data.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/__pycache__/get_send_data.cpython-36.pyc -------------------------------------------------------------------------------- /Robot_Program/__pycache__/plot_graph_2.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/__pycache__/plot_graph_2.cpython-36.pyc -------------------------------------------------------------------------------- /Robot_Program/axes_robot.py: -------------------------------------------------------------------------------- 1 | # This file acts as configuration file for robot you are using 2 | # It works in conjustion with configuration file from robotics toolbox 3 | 4 | from numpy import pi 5 | import numpy as np 6 | import matplotlib 7 | import matplotlib.pyplot as plt 8 | import roboticstoolbox as rtb 9 | 10 | 11 | robot = rtb.models.DH.CM6() 12 | Joint_num = 6 # Number of joints 13 | 14 | Joint_limits_ticks =[[-4096,0], [0,7000], [0,7000], [-2048,2048], [-2000,2000], [-2560,2560]] # values you get after homing robot and moving it to its most left and right sides 15 | 16 | Joint_reduction_ratio = [8, 8, 9.142857143, 6.5, 6, 5] # Reduction ratio we have on our joints 17 | 18 | Encoder_resolution =[1024,1024,1024,1024,1024,1024] # S-Drive is using 10 bit encoders = 1024 ticks per revolution 19 | 20 | # These need to be readjusted if you are changing homing position. Zero joint angles WILL ALWAYS NEED TO FOLLOW KINEMATIC DIAGRAM OF THE ROBOT !!! 21 | Joint_offsets = [pi/2, -3.512999 , -1.264317, 0, 0, 0] #[pi/2, -1.264334-pi/2 , 0.3714061]# Here we adjust robot angles to match ones in robotic toolbox. Easiest to adjust when robot is in kinematic zero position 22 | 23 | Direction_offsets = [-1,-1,1,-1,-1,1] # If angle change is true to angle change in kinematic model then this is 1 else 0 24 | 25 | 26 | # Stuff for jogging of single motors 27 | Joint_max_speed = [1.57075, 1.57075, 1.57075, 1.57075, 1.57075, 1.57075] # max speed in RAD/S used, can go to much more than 1.57 but there is bug in S-drive firmware 28 | Joint_min_speed = [0.02617993875, 0.02617993875, 0.02617993875,0.02617993875, 0.02617993875, 0.02617993875] # min speed in RAD/S used 29 | 30 | Joint_max_acc = [5, 5, 5, 5, 5, 5] # max acceleration in RAD/S² 31 | Joint_min_acc = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # min speed in RAD/S used 32 | ############################################# 33 | 34 | Default_current_limits = [2000,4000,2000,2000,2000,2000] # Current limits for each motor in mA. Once motor reaches this limit it goes to safety mode 35 | Default_security_stop_duration = 3 36 | 37 | Data_interval = 0.02 # 0.01 seconds 10 ms 38 | 39 | # default stuff for steer mode 40 | Steer_K1_default = [3, 4, 3, 3, 3, 3] 41 | Steer_K2_default = [0, 0, 0, 0, 0, 0] 42 | Steer_K3_default = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 43 | Steer_K4_default= [2500, 5000, 2500, 2500, 2500, 2500] 44 | 45 | 46 | def RAD2E(True_Radians,index): 47 | 48 | ''' Transform True radian value of joints to raw encoder value of joints. 49 | Note that radian values are usually offset and changed direction from encoder values 50 | This is done so that radians match kinematic model and right hand positive direction rule ''' 51 | #return_var = (True_Radians * Encoder_resolution[index] * Joint_reduction_ratio[index] - Direction_offsets[index] * Joint_offsets[index]) / (Direction_offsets[index] * 2 * pi) 52 | return_var = (True_Radians * Encoder_resolution[index] * Joint_reduction_ratio[index] - Direction_offsets[index] * Joint_offsets[index]* Encoder_resolution[index] * Joint_reduction_ratio[index]) / (Direction_offsets[index] * 2 * pi ) 53 | 54 | if isinstance(return_var,np.ndarray): 55 | return return_var.astype(int) 56 | else: 57 | return int(return_var) 58 | 59 | 60 | def E2RAD(Motor_Encoder,index): 61 | 62 | ''' Transform raw encoder value to true radian value of joints. 63 | Note that radian values are usually offset and changed direction from encoder values 64 | This is done so that radians match kinematic model and right hand positive direction rule ''' 65 | 66 | return_var = ((Motor_Encoder * 2/(Encoder_resolution[index] * Joint_reduction_ratio[index]) * pi + Joint_offsets[index]) * Direction_offsets[index]) 67 | 68 | return return_var 69 | 70 | 71 | def RAD2D(True_Radians): 72 | 73 | ''' Transform true radian values to true degrees value ''' 74 | return_var = np.rad2deg(True_Radians) 75 | 76 | return return_var 77 | 78 | 79 | def RPM2RADS(Motor_RPM,index): 80 | 81 | ''' Transform raw RPM value we receive from controllers to RAD/S value. 82 | Note that RPM value we receive is WITHOUT REDUCER. 83 | RAD/S value is value WITH reducer. ''' 84 | 85 | return_var = (Motor_RPM / Joint_reduction_ratio[index]) * (pi/30) 86 | 87 | return return_var 88 | 89 | 90 | def RADS2RPM(True_RADS,index): 91 | 92 | ''' Transform true RAD/S value to raw RPM of motors. 93 | Note that raw RPM of motors is without gear reduction ''' 94 | 95 | return_var = (True_RADS * Joint_reduction_ratio[index] * 30) / pi 96 | 97 | return return_var 98 | 99 | 100 | def RADS2_true_RPM(True_RADS): 101 | 102 | ''' Transform true RADS/S to true RPM. 103 | Both these values are true values at witch motor joints move ''' 104 | 105 | return_var = (True_RADS * 30) / pi 106 | 107 | return return_var 108 | 109 | def GO_TO_POSE(pose1,pose2,exec_time): 110 | 111 | speed_true_togo = [0] * Joint_num 112 | pos_true_togo = [0] * Joint_num 113 | for i in range(0,Joint_num): 114 | temp_var = pose2[i] - pose1[i] 115 | speed_true_togo[i] = temp_var / exec_time 116 | speed_true_togo[i] = abs(RADS2RPM(speed_true_togo[i],i)) 117 | pos_true_togo[i] = RAD2E(pose2[i],i) 118 | 119 | return pos_true_togo, speed_true_togo 120 | 121 | 122 | def CSAAR(pose1,pose2,exec_time): 123 | 124 | """ This function creates Continous speed angle to angle rotation for every joint 125 | It returns numpy array with joint angles that change from 0 to exec_time every 10ms """ 126 | 127 | Number_of_Steps = exec_time / Data_interval 128 | Increment_steps = np.empty(shape=6,order='C',dtype='object') 129 | for n in range(0,6): 130 | Increment_steps[n] = (pose2[n] - pose1[n]) / Number_of_Steps 131 | 132 | Out_traj = np.empty(shape=(int(Number_of_Steps),6),order='C',dtype='object') 133 | 134 | for n in range(0, int(Number_of_Steps)): 135 | for m in range(0 ,6): 136 | Out_traj[n][m] = pose1[m] + Increment_steps[m] * (n + 1) 137 | 138 | return Out_traj 139 | 140 | 141 | def Clean_CTRAJ(pose1,pose2,exec_time): 142 | 143 | T1 = (robot.fkine(pose1)) 144 | T2 = (robot.fkine(pose2)) 145 | Num_steps = int(exec_time / Data_interval) 146 | 147 | TT2 = rtb.tools.trajectory.ctraj(T1, T2, Num_steps) 148 | 149 | qx1 = robot.ikine_LM(TT2) 150 | qx1_q = [0] * Num_steps 151 | 152 | for x in range(0,Num_steps): 153 | qx1_q[x] = qx1[x].q 154 | 155 | return qx1_q 156 | 157 | 158 | if __name__ == "__main__": 159 | 160 | var = Clean_CTRAJ([0,np.pi/2,0,0.01,0.50,0],[0.31,0.5,2.0,0.0,0.3,0.70],2) 161 | plt.plot(var) 162 | plt.show() 163 | #print(var) 164 | 165 | -------------------------------------------------------------------------------- /Robot_Program/blue_arrow_left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/blue_arrow_left.png -------------------------------------------------------------------------------- /Robot_Program/blue_arrow_right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/blue_arrow_right.png -------------------------------------------------------------------------------- /Robot_Program/ctraj_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | @author Jesse Haviland 4 | """ 5 | 6 | import roboticstoolbox as rtb 7 | from spatialmath import * 8 | import matplotlib 9 | import matplotlib.pyplot as plt 10 | import numpy as np 11 | import time 12 | 13 | fig, ax = plt.subplots(1, 2) 14 | robot = rtb.models.DH.Puma560() 15 | 16 | print(robot.isspherical()) 17 | 18 | 19 | q1 = np.array([0,0,0,0,0,0]) 20 | 21 | q2 = np.array([0.1,0,0,0,0,0]) 22 | 23 | T1 = (robot.fkine(q1)) 24 | print(T1) 25 | T2 = (robot.fkine(q2)) 26 | print(T2) 27 | var = robot.ikine(T1) 28 | print(var) 29 | var = robot.ikine(T2) 30 | 31 | print(var) 32 | 33 | 34 | 35 | #ls = rtb.tools.trajectory.lspb(q1, q2, 100, V=None) 36 | qt2 = rtb.tools.trajectory.ctraj(T1, T2, 20) 37 | 38 | v = np.array(q1) 39 | print( v) 40 | #v = np.atleast_2d(v).T 41 | print( qt2) 42 | 43 | t1 = time.time() 44 | var = robot.ikine(qt2) 45 | #var = robot.ikinem(qt2[49]) 46 | var = robot.ikine(qt2) 47 | print(var) 48 | 49 | 50 | 51 | ax[0].set_ylabel('Position [RAD]') 52 | #ax[1].set_ylabel('Speed in [RAD/S]') 53 | #ax[2].set_ylabel('Acceleration [RAD/S**2') 54 | 55 | ax[0].plot(var ,linewidth=1.5) #DEG = RAD * (180 / np.pi) 56 | #ax[1].plot(qt2 ,linewidth=1.5) # RPM = rad/s * (60/(2*np.pi)) 57 | #ax[2].plot(qt.qdd,linewidth=1.5) 58 | 59 | plt.legend(["joint1","joint2","joint3","joint4","joint5","joint6"],bbox_to_anchor=(0., 1.02, 1., .102), loc='lower left', ncol=1, borderaxespad=0.) 60 | plt.show() 61 | 62 | 63 | #robot.plot(qt.q.T, dt=10, block=True) #movie='puma_sitting.gif') 64 | #robot.plot(qt2, dt=10, block=False,movie='puma_sitting_2.gif') 65 | -------------------------------------------------------------------------------- /Robot_Program/disable_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/disable_img.png -------------------------------------------------------------------------------- /Robot_Program/enable_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/enable_img.png -------------------------------------------------------------------------------- /Robot_Program/get_send_data.py: -------------------------------------------------------------------------------- 1 | """ It can send to individual motors by adding joint prefix over commands: 2 | Modes and commands available: 3 | #### Commands need to be sent correctly or else it will be ignored. #### 4 | #### replace words with numbers.Kp and speed can be float values! 5 | 6 | 1 - Go to position and hold: 7 | h(position),speed,Kp,current_threshold 8 | example: h100,20,3.1,12 9 | 10 | 2 - Speed to position and sent flag 11 | s(position),speed 12 | 13 | 3 - Gravitiy compensation mode 14 | g(current_threshold),compliance_speed 15 | 16 | 4 - Position hold mode 17 | p(Kp),current_threshold 18 | 19 | 5 - Speed mode with direction 20 | o(direction 0 or 1),speed 21 | 22 | 6 - Jump to position 23 | j(position),Kp,current_threshold 24 | 25 | 7 - Voltage mode 26 | v(direction 0 or 1),voltage(0-1000) 27 | 28 | 8 - Disable motor 29 | d 30 | 31 | 9 - Enable motor 32 | e 33 | 34 | 10 - Clear error 35 | c 36 | 37 | 11 - Change motor data 38 | i(Error_temperature),Error_current,Serial_data_outpu_interval 39 | 40 | 12 - teleoperation mode 41 | x(position),speed,K1_t,K2_t,K3_t,K4_t 42 | // K1_t is most important, K2_t is for speed while 43 | // K3_t and K4_t are for current but they tend to make whole system unstable so be carefull 44 | // TO disable K3_t enter value 0, to disable K4_t enter value !!!!LARGER!!!! then short circuit current!!! 45 | 46 | Now if no commands is to be sent to motors, Joint level data sender needs to send dummy code. 47 | If dummy code is not sent controller will report error and probably send motors to gravity compensation. """ 48 | 49 | 50 | import serial as sr 51 | import time 52 | import numpy as np 53 | import axes_robot as rbt 54 | 55 | s = sr.Serial(timeout = None) 56 | s.baudrate = 10e6 57 | s.port = '/dev/ttyACM0' 58 | # If there is no serial device available on '/dev/ttyACM0' software will not run 59 | s.open() #Comment this out if you want to run the software without device connected 60 | print(s.name) 61 | 62 | 63 | def send_dummy_data(): 64 | s.write(b'#') 65 | s.write(b'\n') 66 | 67 | def GOTO_position_HOLD(Joint_, Position_, Speed_, Kp_, Current_): 68 | 69 | s.write(b'h') 70 | s.write(bytes(str(Joint_), encoding="ascii")) 71 | s.write(bytes(str(int(Position_)), encoding="ascii")) 72 | s.write(b',') 73 | s.write(bytes(str(round(Speed_,4)), encoding="ascii")) 74 | s.write(b',') 75 | s.write(bytes(str(round(Kp_,2)), encoding="ascii")) 76 | s.write(b',') 77 | s.write(bytes(str(int(Current_)), encoding="ascii")) 78 | s.write(b'\n') 79 | 80 | def Speed_Flag(Joint_, Position_, Speed_): 81 | 82 | s.write(b's') 83 | s.write(bytes(str(Joint_), encoding="ascii")) 84 | s.write(bytes(str(int(Position_)), encoding="ascii")) 85 | s.write(b',') 86 | s.write(bytes(str(round(Speed_,4)), encoding="ascii")) 87 | s.write(b'\n') 88 | 89 | def Gravity_compensation(Joint_, Current_, Comp_): 90 | 91 | s.write(b'g') 92 | s.write(bytes(str(Joint_), encoding="ascii")) 93 | s.write(bytes(str(int(Current_)), encoding="ascii")) 94 | s.write(b',') 95 | s.write(bytes(str(int(Comp_)), encoding="ascii")) 96 | s.write(b'\n') 97 | 98 | def Disable(Joint_): 99 | 100 | s.write(b'd') 101 | s.write(bytes(str(Joint_), encoding="ascii")) 102 | s.write(b'\n') 103 | 104 | def Enable(Joint_): 105 | 106 | s.write(b'e') 107 | s.write(bytes(str(Joint_), encoding="ascii")) 108 | s.write(b'\n') 109 | 110 | def Strong_position_hold(Joint_, Kp_, Current_): 111 | 112 | s.write(b'p') 113 | s.write(bytes(str(Joint_), encoding="ascii")) 114 | s.write(bytes(str(round(Kp_,3)), encoding="ascii")) 115 | s.write(b',') 116 | s.write(bytes(str(int(Current_)), encoding="ascii")) 117 | s.write(b'\n') 118 | 119 | def Speed_Dir(Joint_, Dir_, Speed_): 120 | 121 | s.write(b'o') 122 | s.write(bytes(str(Joint_), encoding="ascii")) 123 | s.write(bytes(str(Dir_), encoding="ascii")) 124 | s.write(b',') 125 | s.write(bytes(str(round(Speed_,4)), encoding="ascii")) 126 | s.write(b'\n') 127 | 128 | def Voltage_Mode(Joint_, Dir_, Voltage_): 129 | 130 | s.write(b'v') 131 | s.write(bytes(str(Joint_), encoding="ascii")) 132 | s.write(bytes(str(Dir_), encoding="ascii")) 133 | s.write(b',') 134 | s.write(bytes(str(int(Voltage_)), encoding="ascii")) 135 | s.write(b'\n') 136 | 137 | def Clear_Error(Joint_): 138 | 139 | s.write(b'c') 140 | s.write(bytes(str(Joint_), encoding="ascii")) 141 | s.write(b'\n') 142 | 143 | def Jump_position(Joint_, Position_, Kp_, Current_): 144 | 145 | s.write(b'j') 146 | s.write(bytes(str(Joint_), encoding="ascii")) 147 | s.write(bytes(str(int(Position_)), encoding="ascii")) 148 | s.write(b',') 149 | s.write(bytes(str(round(Kp_,3)), encoding="ascii")) 150 | s.write(b',') 151 | s.write(bytes(str(int(Current_)), encoding="ascii")) 152 | s.write(b'\n') 153 | 154 | def Change_data(Joint_, E_temp, E_Current, Serial_data_output_interval): 155 | 156 | s.write(b'i') 157 | s.write(bytes(str(Joint_), encoding="ascii")) 158 | s.write(bytes(str(int(E_temp)), encoding="ascii")) 159 | s.write(b',') 160 | s.write(bytes(str(int(E_Current)), encoding="ascii")) 161 | s.write(b',') 162 | s.write(bytes(str(int(Serial_data_output_interval)), encoding="ascii")) 163 | s.write(b'\n') 164 | 165 | def teleop_mode(Joint_,Position_,Speed_,K1_t,K2_t,K3_t,K4_t): 166 | s.write(b'x') 167 | s.write(bytes(str(Joint_), encoding="ascii")) 168 | s.write(bytes(str(int(Position_)), encoding="ascii")) 169 | s.write(b',') 170 | s.write(bytes(str(round(Speed_,2)), encoding="ascii")) 171 | s.write(b',') 172 | s.write(bytes(str(round(K1_t,3)), encoding="ascii")) 173 | s.write(b',') 174 | s.write(bytes(str(round(K2_t,3)), encoding="ascii")) 175 | s.write(b',') 176 | s.write(bytes(str(round(K3_t,3)), encoding="ascii")) 177 | s.write(b',') 178 | s.write(bytes(str(round(K4_t,3)), encoding="ascii")) 179 | s.write(b'\n') 180 | 181 | 182 | 183 | #initiate arrays 184 | position_var = [None] * rbt.Joint_num 185 | position_var_RADS = [None] * rbt.Joint_num 186 | speed_var = [None] * rbt.Joint_num 187 | current_var = [None] * rbt.Joint_num 188 | temperature_var = [None] * rbt.Joint_num 189 | speed_var_RADS = [None] * rbt.Joint_num 190 | 191 | 192 | def get_data(data_rec_): 193 | 194 | ''' data gets received in this order: position,current,speed,temperature,voltage,error ''' 195 | 196 | data_split = data_rec_.split(b',') # Data split splits all data on "," and places it in array 197 | 198 | # Fill arrays with data points received 199 | for x in range(rbt.Joint_num): 200 | 201 | position_var[x] = int(data_split[x].decode("utf-8")) 202 | position_var_RADS[x] = rbt.E2RAD(position_var[x],x) 203 | current_var[x] = int(data_split[x + rbt.Joint_num].decode("utf-8")) 204 | speed_var[x] = int(data_split[x + rbt.Joint_num * 2].decode("utf-8")) 205 | speed_var_RADS[x] = rbt.RPM2RADS(speed_var[x],x) 206 | temperature_var[x] = int(data_split[x + rbt.Joint_num * 3].decode("utf-8")) 207 | 208 | voltage_var = int(data_split[4 * rbt.Joint_num].decode("utf-8")) 209 | error_var = int(data_split[4 * rbt.Joint_num + 1].decode("utf-8")) 210 | 211 | return position_var, position_var_RADS, current_var, speed_var, speed_var_RADS, temperature_var, voltage_var, error_var 212 | 213 | 214 | 215 | def main_comms_func(): 216 | data_rec = s.readline() 217 | #print(data_rec) 218 | d1,d2,d3,d4,d5,d6,d7,d8 = get_data(data_rec) 219 | 220 | return d1,d2,d3,d4,d5,d6,d7,d8 221 | 222 | 223 | 224 | def try_reconnect(): 225 | try: 226 | s.close() 227 | time.sleep(0.01) 228 | s.open() 229 | time.sleep(0.01) 230 | except: 231 | print("no serial available") 232 | 233 | 234 | if __name__ == "__main__": 235 | #Enable(2) 236 | #Clear_Error(2) 237 | while(1): 238 | try: 239 | #bg = time.time() 240 | a1,a2,a3,a4,a5,a6,a7,a8 = main_comms_func() 241 | #print(time.time() - bg) 242 | #print(a1) 243 | #print(a2) 244 | #print(a3) 245 | #time.sleep(0.01) 246 | #teleop_mode(2,2000,0,18,0,0.7,1000) # 0.7, 1000 247 | #Disable(2) 248 | except: 249 | try_reconnect() 250 | 251 | 252 | -------------------------------------------------------------------------------- /Robot_Program/helpimg4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/helpimg4.png -------------------------------------------------------------------------------- /Robot_Program/plot_graph_2.py: -------------------------------------------------------------------------------- 1 | 2 | """ Writen by: Petar Crnjak 3 | Date: 20.10.2020 4 | Tested on: Python 3.8.5 5 | matplotlib 3.3.2 """ 6 | 7 | import matplotlib 8 | import matplotlib.pyplot as plt 9 | from matplotlib.animation import FuncAnimation 10 | 11 | # All these variables should be multi-process variables 12 | # Proc_plot_show tells us what plot we are showing i.e. position, speed, current, temperature 13 | # 0 is position, 1 is speed, 2 is current and 3 is temperature! 14 | # If close_event is 1 animation is closed if it is 0 it is open and running 15 | # When plot is closed directly (By pressing X), under runGraph() you should set close_event multi-process variable to 1 16 | # If you dont do that when you close plot with "X" it will re-open itself 17 | # proc_position, speed, current and temperature are lists from 1-7 elements. 18 | # Plot will always open at same position on screen and with same size. 19 | # Once opened you can resize and move it! 20 | # runGraph is loop by itself but once we exit it we cant re run it unless we are in another loop 21 | # since it is written to be used in multi-procces aplications you need to place runGraph in external loop 22 | # That loop will then control when it is opened and closed 23 | 24 | def runGraph(proc_position, proc_speed, proc_current, proc_temperature, proc_plot_show, close_event): 25 | 26 | # This function selects what labels to show 27 | def showing_plot(): 28 | 29 | # if plot is position / joint angles 30 | if proc_plot_show == 0: 31 | 32 | y_range = [-4, 4] # What range to show on Y axis 33 | ax.set_ylim(y_range) 34 | plt.xlabel('Samples [10ms] ', weight='bold') 35 | plt.ylabel('Joint angle [deg]', weight='bold') 36 | plt.legend(legend_print,bbox_to_anchor=(0., 1.02, 1., .102), loc='lower left', ncol=2, borderaxespad=0.) 37 | 38 | plt.grid(True) 39 | plt.tight_layout() 40 | 41 | # if plot is speed 42 | elif proc_plot_show == 1: 43 | 44 | y_range = [-10.0, 10.0] # What range to show on Y axis 45 | ax.set_ylim(y_range) 46 | plt.xlabel('Samples [10ms] ', weight='bold') 47 | plt.ylabel('Joint speed [RPM]', weight='bold') 48 | plt.legend(legend_print,bbox_to_anchor=(0., 1.02, 1., .102), loc='lower left', ncol=2, borderaxespad=0.) 49 | 50 | plt.grid(True) 51 | plt.tight_layout() 52 | 53 | #if plot is current 54 | elif proc_plot_show == 2: 55 | 56 | y_range = [0, 2000] # What range to show on Y axis 57 | ax.set_ylim(y_range) 58 | plt.xlabel('Samples [10ms] ', weight='bold') 59 | plt.ylabel('Joint curent [RPM]', weight='bold') 60 | plt.legend(legend_print,bbox_to_anchor=(0., 1.02, 1., .102), loc='lower left', ncol=2, borderaxespad=0.) 61 | 62 | plt.grid(True) 63 | plt.tight_layout() 64 | 65 | elif proc_plot_show == 3: 66 | 67 | y_range = [-5, 90] # What range to show on Y axis 68 | ax.set_ylim(y_range) 69 | plt.xlabel('Samples [10ms] ', weight='bold') 70 | plt.ylabel('Joint temperature [RPM]', weight='bold') 71 | plt.legend(legend_print,bbox_to_anchor=(0., 1.02, 1., .102), loc='lower left', ncol=2, borderaxespad=0.) 72 | 73 | plt.grid(True) 74 | plt.tight_layout() 75 | 76 | 77 | matplotlib.use("TkAgg") # set the backend 78 | x_len = 1000 # Number of points to display on X axis 79 | plt.style.use('bmh') 80 | data_len = len(proc_position) # Check length of data touple. 81 | line = [None] * 7 82 | 83 | ys1 = [0] * x_len 84 | ys2 = [0] * x_len 85 | ys3 = [0] * x_len 86 | ys4 = [0] * x_len 87 | ys5 = [0] * x_len 88 | ys6 = [0] * x_len 89 | ys7 = [0] * x_len 90 | 91 | color_range = ["r", "g", "b", "y", "m", "k", "c"] 92 | 93 | # https://stackoverflow.com/questions/28575192/how-do-i-set-the-matplotlib-window-size-for-the-macosx-backend 94 | fig = plt.figure(figsize=(5, 3)) 95 | # https://stackoverflow.com/questions/7449585/how-do-you-set-the-absolute-position-of-figure-windows-with-matplotlib 96 | fig.canvas.manager.window.wm_geometry("+%d+%d" % (1500, 700)) 97 | #fig.canvas.manager.window.attributes('-topmost', 1) 98 | #fig.canvas.manager.window.set_keep_above 99 | 100 | #fig.canvas.toolbar.pack_forget() 101 | ax = fig.add_subplot(1, 1, 1) 102 | xs = list(range(0, x_len)) 103 | 104 | legend_print = [] # What data we will print in legend 105 | 106 | for x in range(data_len): 107 | if proc_plot_show == 0: 108 | legend_print.append("Pos" + str(x+1)) 109 | elif proc_plot_show == 1: 110 | legend_print.append("Spd" + str(x+1)) 111 | elif proc_plot_show == 2: 112 | legend_print.append("Cur" + str(x+1)) 113 | elif proc_plot_show == 3: 114 | legend_print.append("Temp" + str(x+1)) 115 | 116 | 117 | # Creating blank lines 118 | line[0], = ax.plot(xs, ys1, color = color_range[0]) 119 | line[1], = ax.plot(xs, ys2, color = color_range[1]) 120 | line[2], = ax.plot(xs, ys3, color = color_range[2]) 121 | line[3], = ax.plot(xs, ys4, color = color_range[3]) 122 | line[4], = ax.plot(xs, ys5, color = color_range[4]) 123 | line[5], = ax.plot(xs, ys6, color = color_range[5]) 124 | line[6], = ax.plot(xs, ys7, color = color_range[6]) 125 | 126 | data_show = [] 127 | showing_plot() 128 | 129 | # This function is called periodically from FuncAnimation 130 | def on_close(event): 131 | event.canvas.figure.axes[0].has_been_closed = True 132 | print ('Closed Figure with X') 133 | 134 | def data_append(): 135 | 136 | if proc_plot_show == 0: 137 | data_show = proc_position 138 | elif proc_plot_show == 1: 139 | data_show = proc_speed 140 | elif proc_plot_show == 2: 141 | data_show = proc_current 142 | elif proc_plot_show == 3: 143 | data_show = proc_temperature 144 | 145 | if data_len == 1: 146 | ys1.append(data_show[0]) 147 | elif data_len == 2: 148 | ys1.append(data_show[0]) 149 | ys2.append(data_show[1]) 150 | elif data_len == 3: 151 | ys1.append(data_show[0]) 152 | ys2.append(data_show[1]) 153 | ys3.append(data_show[2]) 154 | elif data_len == 4: 155 | ys1.append(data_show[0]) 156 | ys2.append(data_show[1]) 157 | ys3.append(data_show[2]) 158 | ys4.append(data_show[3]) 159 | elif data_len == 5: 160 | ys1.append(data_show[0]) 161 | ys2.append(data_show[1]) 162 | ys3.append(data_show[2]) 163 | ys4.append(data_show[3]) 164 | ys5.append(data_show[4]) 165 | elif data_len == 6: 166 | ys1.append(data_show[0]) 167 | ys2.append(data_show[1]) 168 | ys3.append(data_show[2]) 169 | ys4.append(data_show[3]) 170 | ys5.append(data_show[4]) 171 | ys6.append(data_show[5]) 172 | elif data_len == 7: 173 | ys1.append(data_show[0]) 174 | ys2.append(data_show[1]) 175 | ys3.append(data_show[2]) 176 | ys4.append(data_show[3]) 177 | ys5.append(data_show[4]) 178 | ys6.append(data_show[5]) 179 | ys7.append(data_show[6]) 180 | 181 | def animate(i, ys1, ys2, ys3, ys4, ys5, ys6, ys7): 182 | 183 | data_append() 184 | 185 | ys1 = ys1[-x_len:] 186 | ys2 = ys2[-x_len:] 187 | ys3 = ys3[-x_len:] 188 | ys4 = ys4[-x_len:] 189 | ys5 = ys5[-x_len:] 190 | ys6 = ys6[-x_len:] 191 | ys7 = ys7[-x_len:] 192 | 193 | fig.canvas.mpl_connect('close_event', on_close) 194 | 195 | if close_event == 1: 196 | ani.event_source.stop() 197 | 198 | if data_len == 0: 199 | line[0].set_ydata(ys1) 200 | elif data_len == 1: 201 | line[0].set_ydata(ys1) 202 | line[1].set_ydata(ys2) 203 | elif data_len == 2: 204 | line[0].set_ydata(ys1) 205 | line[1].set_ydata(ys2) 206 | line[2].set_ydata(ys3) 207 | elif data_len == 3: 208 | line[0].set_ydata(ys1) 209 | line[1].set_ydata(ys2) 210 | line[2].set_ydata(ys3) 211 | line[3].set_ydata(ys4) 212 | elif data_len == 4: 213 | line[0].set_ydata(ys1) 214 | line[1].set_ydata(ys2) 215 | line[2].set_ydata(ys3) 216 | line[3].set_ydata(ys4) 217 | line[4].set_ydata(ys5) 218 | elif data_len == 5: 219 | line[0].set_ydata(ys1) 220 | line[1].set_ydata(ys2) 221 | line[2].set_ydata(ys3) 222 | line[3].set_ydata(ys4) 223 | line[4].set_ydata(ys5) 224 | line[5].set_ydata(ys6) 225 | elif data_len == 6: 226 | line[0].set_ydata(ys1) 227 | line[1].set_ydata(ys2) 228 | line[2].set_ydata(ys3) 229 | line[3].set_ydata(ys4) 230 | line[4].set_ydata(ys5) 231 | line[5].set_ydata(ys6) 232 | line[6].set_ydata(ys7) 233 | 234 | 235 | return line[0], line[1], line[2], line[3], line[4], line[5], line[6] 236 | 237 | ani = FuncAnimation(fig, 238 | animate, 239 | fargs=(ys1, ys2, ys3, ys4, ys5, ys6, ys7,), 240 | interval=50, 241 | blit=True) 242 | 243 | plt.show() 244 | 245 | 246 | if __name__ == "__main__": 247 | close = True 248 | while(1): 249 | #ove variable dobivam od drugog procesa 250 | var1 = [5.213 ,7.312,9.213,3.12,2.13,1.12] 251 | var2 = [0.5 ,0.7,0.9,0.3,0.2,0.100] 252 | var3 = [500 ,700,90,300,200,100] 253 | var4 = [50 ,70,90,30,20,10] 254 | #close = True 255 | if close == True : 256 | # sve ove variable koje tu prima su multi_proc varijable 257 | # runGraph(proc_position, proc_speed, proc_current, proc_temperature, proc_plot_show, close_event): 258 | runGraph(var1,var2,var3,var4,0,0) # event close zatvara kada je 1 259 | # u proces loopu kada dodem do ove linije kazem da je close_event globalna varijabla = 1 tako da se 260 | # graf više ne otvara 261 | #print(ev) 262 | print("run") 263 | #close = False -------------------------------------------------------------------------------- /Robot_Program/plot_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | @author Jesse Haviland 4 | """ 5 | 6 | import roboticstoolbox as rp 7 | import matplotlib 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | import time 11 | import axes_robot as rb 12 | 13 | fig, ax = plt.subplots(1, 3) 14 | 15 | # Make a panda robot 16 | link3 = rp.models.DH.three_link() 17 | 18 | q0 = [0.1, 0.5, 0.3] 19 | qf = [1.2, 1.1, 1] 20 | 21 | s1 = time.time() 22 | t2 = (np.arange(0,3.5,0.01)) 23 | #print(len(t2)) 24 | s_speed = np.array([0.3,0.5,0.6]) 25 | qt = rp.tools.trajectory.jtraj(q0, qf, t2,s_speed,s_speed) 26 | test = (rb.RAD2E((qt.q[:,0]),0)) 27 | #print(test) 28 | #q_test = rb.RAD2E(qt.q 29 | s2 = time.time() 30 | 31 | print(s2-s1) 32 | #fig.suptitle('Position, speed and acceleration plots') 33 | 34 | ax[0].set_ylabel('Position [DEG]') 35 | ax[1].set_ylabel('Speed in [RPM]') 36 | ax[2].set_ylabel('Acceleration [RAD/S**2') 37 | 38 | #print(rb.RAD2E((qt.q[:,0]),0)) 39 | print(type(qt.q)) 40 | print(len(qt.q)) 41 | print((qt.q).shape) 42 | #ax1.plot(qt.q[:,2]) 43 | #ax1.plot(qt.q[:,1]) 44 | ax[0].plot(qt.q * (180/np.pi),linewidth=1.5) #DEG = RAD * (180 / np.pi) 45 | ax[1].plot(qt.qd * (60/(2*np.pi)),linewidth=1.5) # RPM = rad/s * (60/(2*np.pi)) 46 | ax[2].plot(qt.qdd,linewidth=1.5) 47 | 48 | #plt.legend(["joint","joint2","joint3"],bbox_to_anchor=(0., 1.02, 1., .102), loc='lower left', ncol=1, borderaxespad=0.) 49 | #plt.show() 50 | 51 | link3.plot(qt.q.T, dt=10 ) 52 | 53 | #link3.q = link3.qs 54 | # Init joint to the 'ready' joint angles 55 | #panda.q = panda.qz 56 | 57 | # Open a plot with the teach panel 58 | #e = link3.teach() -------------------------------------------------------------------------------- /Robot_Program/python_tests.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | 4 | clean_string = [] 5 | text_file = open("/home/rope/Desktop/Robot_Program/Programs/execute_script.txt",'r+') 6 | code_string = text_file.readlines() # this is list where each index is one line of code. Each index needs to be aditionaly processed 7 | code_len = len(code_string) 8 | #data_split = code_string.split(b',') 9 | #print(data_split) 10 | 11 | text_file.close() 12 | valid_data = 0 13 | 14 | # data = [1,2,3,4,5,6,7] 15 | # if __name__ == "__main__": 16 | 17 | # print("here") 18 | # for i in range(0,len(data)): 19 | # if data[i] != 5: 20 | # clean_string.append(data[i]) 21 | # else: 22 | # continue 23 | 24 | # print(data[i]) 25 | # print(clean_string) 26 | 27 | if __name__ == "__main__": 28 | 29 | # this code cleans string data 30 | for i in range(0,code_len): 31 | if code_string[i] == '\n': 32 | continue 33 | else: 34 | clean_string.append(code_string[i]) 35 | 36 | if clean_string[len(clean_string)-1] == 'end\n' or clean_string[len(clean_string)-1] == 'loop\n': 37 | valid_data = 1 38 | else: 39 | valid_data = 0 40 | 41 | ##### 42 | 43 | print(valid_data) 44 | print(clean_string) 45 | 46 | code2execute = clean_string[0].split(',') 47 | code2execute = code2execute[:-1] 48 | if(code2execute[0] == 'pos'): 49 | print("1") 50 | elif(code2execute[0] == 'delay'): 51 | print("2") 52 | elif(code2execute[0] == 'end'): 53 | print("3") 54 | 55 | print(code2execute) 56 | 57 | #print(int('0.5\n')) 58 | 59 | a = [1, 2, 3, 4, 3] 60 | b = [9, 8, 7, 6, 5] 61 | if np.any(np.array(a)>np.array(b)): 62 | print("yes") 63 | else: 64 | print("no") 65 | 66 | # print(set(a) & set(b)) 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /Robot_Program/robotic_toolbox_CM6_models/CM6.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | @author: Peter Corke 4 | """ 5 | 6 | # Note:: 7 | # - SI units are used. 8 | # - Gear ratios not currently known, though reflected armature inertia 9 | # is known, so gear ratios are set to 1. 10 | # 11 | # References:: 12 | # - Kinematic data from "Modelling, Trajectory calculation and Servoing of 13 | # a computer controlled arm". Stanford AIM-177. Figure 2.3 14 | # - Dynamic data from "Robot manipulators: mathematics, programming and 15 | # control" 16 | # Paul 1981, Tables 6.5, 6.6 17 | # - Dobrotin & Scheinman, "Design of a computer controlled manipulator for 18 | # robot research", IJCAI, 1973. 19 | 20 | # all parameters are in SI units: m, radians, kg, kg.m2, N.m, N.m.s etc. 21 | 22 | from roboticstoolbox import DHRobot, RevoluteDH, PrismaticDH 23 | from math import pi 24 | import numpy as np 25 | 26 | 27 | class CM6(DHRobot): 28 | """ 29 | Create model of Stanford arm manipulator 30 | 31 | puma = Puma560() is a script which creates a puma SerialLink object 32 | describing the kinematic and dynamic characteristics of a Unimation Puma 33 | 560 manipulator using standard DH conventions. 34 | 35 | Also define some joint configurations: 36 | - qz, zero joint angle configuration, 'L' shaped configuration 37 | - qr, vertical 'READY' configuration 38 | - qs, arm is stretched out in the X direction 39 | - qn, arm is at a nominal non-singular configuration 40 | """ 41 | 42 | def __init__(self): 43 | 44 | deg = pi/180 45 | inch = 0.0254 46 | 47 | L0 = RevoluteDH( 48 | d=0.16265, # link length (Dennavit-Hartenberg notation) 49 | a=0, # link offset (Dennavit-Hartenberg notation) 50 | alpha=pi/2, # link twist (Dennavit-Hartenberg notation) 51 | # inertia tensor of link with respect to 52 | # center of mass I = [L_xx, L_yy, L_zz, 53 | # L_xy, L_yz, L_xz] 54 | I=[0.276, 0.255, 0.071, 0, 0, 0], 55 | # distance of ith origin to center of mass [x,y,z] 56 | # in link reference frame 57 | r=[0, 0.0175, -0.1105], 58 | m=9.29, # mass of link 59 | Jm=0.953, # actuator inertia 60 | G=1, # gear ratio 61 | qlim=[-190*deg, 190*deg]) # minimum and maximum joint angle 62 | 63 | L1 = RevoluteDH( 64 | d=0, a=0.28, alpha = 0, 65 | I=[0.108, 0.018, 0.100, 0, 0, 0], 66 | r=[0, -1.054, 0], 67 | m=5.01, Jm=2.193, G=1, 68 | qlim=[-190*deg, 190*deg]) 69 | 70 | L2 = RevoluteDH( 71 | d=0, a=0, alpha = pi/2, 72 | I=[2.51, 2.51, 0.006, 0, 0, 0], 73 | r=[0, 0, -6.447], 74 | m=4.25, Jm=0.782, G=1, 75 | qlim=[-190*deg, 190*deg]) 76 | 77 | L3 = RevoluteDH( 78 | d=0.25, a=0, alpha=-pi/2, 79 | I=[0.002, 0.001, 0.001, 0, 0, 0], 80 | r=[0, 0.092, -0.054], 81 | m=1.08, Jm=0.106, G=1, 82 | qlim=[-190*deg, 190*deg]) 83 | 84 | L4 = RevoluteDH( 85 | d=0, a=0, alpha=pi/2, 86 | I=[0.003, 0.0004, 0, 0, 0, 0], 87 | r=[0, 0.566, 0.003], m=0.630, 88 | Jm=0.097, G=1, 89 | qlim=[-190*deg, 190*deg]) 90 | 91 | L5 = RevoluteDH( 92 | d=0.0372, a=0, alpha=0, 93 | I=[0.013, 0.013, 0.0003, 0, 0, 0], 94 | r=[0, 0, 1.554], m=0.51, Jm=0.020, 95 | G=1, 96 | qlim=[-190*deg, 190*deg]) 97 | 98 | L = [L0, L1, L2, L3, L4, L5] 99 | 100 | super().__init__( 101 | L, 102 | name="CM6 compliant manipulator", 103 | manufacturer="Petar Crnjak", 104 | keywords=('dynamics',)) 105 | 106 | # zero angles, L shaped pose 107 | self.addconfiguration("qz", np.array([0, np.pi/2, 0, 0, 0, 0])) 108 | 109 | # random pose 110 | self.addconfiguration("qr", np.array([np.pi/3, np.pi/3, -pi/3, np.pi/3, np.pi/3, np.pi/3])) 111 | 112 | 113 | 114 | 115 | if __name__ == '__main__': 116 | 117 | test = CM6() 118 | print(test) 119 | 120 | -------------------------------------------------------------------------------- /Robot_Program/robotic_toolbox_CM6_models/__init__.py: -------------------------------------------------------------------------------- 1 | from roboticstoolbox.models.DH.Panda import Panda 2 | from roboticstoolbox.models.DH.Puma560 import Puma560 3 | from roboticstoolbox.models.DH.Stanford import Stanford 4 | from roboticstoolbox.models.DH.Ball import Ball 5 | from roboticstoolbox.models.DH.Hyper import Hyper 6 | from roboticstoolbox.models.DH.Coil import Coil 7 | from roboticstoolbox.models.DH.Cobra600 import Cobra600 8 | from roboticstoolbox.models.DH.IRB140 import IRB140 9 | from roboticstoolbox.models.DH.KR5 import KR5 10 | from roboticstoolbox.models.DH.Orion5 import Orion5 11 | from roboticstoolbox.models.DH.Planar3 import Planar3 12 | from roboticstoolbox.models.DH.Planar2 import Planar2 13 | from roboticstoolbox.models.DH.LWR4 import LWR4 14 | from roboticstoolbox.models.DH.Sawyer import Sawyer 15 | from roboticstoolbox.models.DH.UR3 import UR3 16 | from roboticstoolbox.models.DH.UR5 import UR5 17 | from roboticstoolbox.models.DH.UR10 import UR10 18 | from roboticstoolbox.models.DH.Mico import Mico 19 | from roboticstoolbox.models.DH.Jaco import Jaco 20 | from roboticstoolbox.models.DH.Baxter import Baxter 21 | from roboticstoolbox.models.DH.TwoLink import TwoLink 22 | from roboticstoolbox.models.DH.Hyper3d import Hyper3d 23 | from roboticstoolbox.models.DH.P8 import P8 24 | from roboticstoolbox.models.DH.CM6 import CM6 25 | from roboticstoolbox.models.DH.three_link import three_link 26 | 27 | __all__ = [ 28 | 'Panda', 29 | 'Puma560', 30 | 'Stanford', 31 | 'Ball', 32 | 'Coil', 33 | 'Hyper', 34 | 'Hyper3d', 35 | 'Cobra600', 36 | 'IRB140', 37 | 'KR5', 38 | 'Orion5', 39 | 'Planar3', 40 | 'Planar2', 41 | 'LWR4', 42 | 'UR3', 43 | 'UR5', 44 | 'UR10', 45 | 'Sawyer', 46 | 'Mico', 47 | 'Jaco', 48 | 'Baxter', 49 | 'TwoLink', 50 | 'P8', 51 | 'CM6', 52 | 'three_link', 53 | ] 54 | -------------------------------------------------------------------------------- /Robot_Program/xdole.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/xdole.png -------------------------------------------------------------------------------- /Robot_Program/xgore.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/xgore.png -------------------------------------------------------------------------------- /Robot_Program/ydesno.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/ydesno.png -------------------------------------------------------------------------------- /Robot_Program/ylevo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/ylevo.png -------------------------------------------------------------------------------- /Robot_Program/zdole.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/zdole.png -------------------------------------------------------------------------------- /Robot_Program/zgore.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PCrnjak/CM6_control_software/3f921b6c82972c3330087dfc513856cf312cdc9c/Robot_Program/zgore.png --------------------------------------------------------------------------------