Rev 1331 | Rev 1333 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1331 | giacomo | 1 | |
| 2 | /* |
||
| 3 | * Project: S.Ha.R.K. |
||
| 4 | * |
||
| 5 | * Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
||
| 6 | * |
||
| 7 | * Authors : Paolo Gai <pj@hartik.sssup.it> |
||
| 8 | * (see authors.txt for full list of hartik's authors) |
||
| 9 | * |
||
| 10 | * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
||
| 11 | * |
||
| 12 | * http://www.sssup.it |
||
| 13 | * http://retis.sssup.it |
||
| 14 | * http://shark.sssup.it |
||
| 15 | */ |
||
| 16 | |||
| 17 | /* |
||
| 18 | * Copyright (C) 2000 Paolo Gai |
||
| 19 | * |
||
| 20 | * This program is free software; you can redistribute it and/or modify |
||
| 21 | * it under the terms of the GNU General Public License as published by |
||
| 22 | * the Free Software Foundation; either version 2 of the License, or |
||
| 23 | * (at your option) any later version. |
||
| 24 | * |
||
| 25 | * This program is distributed in the hope that it will be useful, |
||
| 26 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
| 27 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
| 28 | * GNU General Public License for more details. |
||
| 29 | * |
||
| 30 | * You should have received a copy of the GNU General Public License |
||
| 31 | * along with this program; if not, write to the Free Software |
||
| 32 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
| 33 | * |
||
| 34 | */ |
||
| 35 | |||
| 36 | #include "chimera.h" |
||
| 37 | |||
| 38 | //#define DEBUG_SEND /* Print Sent Values */ |
||
| 39 | #define SERIAL_ON /* Send Data using Serial Port */ |
||
| 40 | |||
| 41 | /* Servo Tasks Constants */ |
||
| 42 | #ifdef DUBUG_SEND |
||
| 43 | #define SEND_TASK_WCET 6000 |
||
| 44 | #else |
||
| 1332 | giacomo | 45 | #define SEND_TASK_WCET 6000 |
| 1331 | giacomo | 46 | #endif |
| 47 | #define SEND_TASK_MIT 20000 |
||
| 48 | |||
| 49 | #ifdef DUBUG_SEND |
||
| 1332 | giacomo | 50 | #define GET_TASK_WCET 6000 |
| 1331 | giacomo | 51 | #else |
| 1332 | giacomo | 52 | #define GET_TASK_WCET 6000 |
| 1331 | giacomo | 53 | #endif |
| 54 | #define GET_TASK_MIT 20000 |
||
| 55 | |||
| 56 | #define LEG_A 100.0 |
||
| 57 | #define LEG_B 66.0 |
||
| 58 | #define LEG_C 26.0 |
||
| 59 | #define LEG_D 38.0 |
||
| 60 | #define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */ |
||
| 61 | #define LEG_CD_ANG 34.380 /* arctg(LEG_C/LEG_D) in gradi */ |
||
| 62 | |||
| 63 | const float c0 = LEG_C * LEG_C; |
||
| 64 | const float c1 = LEG_B * LEG_B; |
||
| 65 | const float c2 = LEG_B * LEG_B - LEG_A * LEG_A; |
||
| 66 | const float todeg = 180.0 / PI; |
||
| 67 | |||
| 68 | sem_t mx_status, mx_servo; |
||
| 69 | HEXAPOD_STATE status; |
||
| 70 | ANGLES_STATE angles[6]; |
||
| 71 | |||
| 1332 | giacomo | 72 | extern struct leg_calibration calibration_table[]; |
| 73 | |||
| 1331 | giacomo | 74 | void print_status(int n){ |
| 75 | int i; |
||
| 76 | |||
| 77 | if (n>=6) { |
||
| 78 | for (i=0; i<6;i++) |
||
| 79 | cprintf("Leg %1d: ( %3d | %3d | %3d ) (%d)\n", i, status.leg[i].x, status.leg[i].y, status.leg[i].z, status.leg[i].pwm ); |
||
| 80 | cprintf("Power: %d", status.power); |
||
| 81 | cprintf("\n"); |
||
| 82 | } else { |
||
| 83 | cprintf("Leg %1d: ( %3d | %3d | %3d ) (%1d-%1d)\n", n, status.leg[n].x,status.leg[n].y,status.leg[n].z, status.leg[n].pwm, status.power); |
||
| 84 | } |
||
| 85 | } |
||
| 86 | |||
| 87 | int set_leg_position(int n, float px, float py, float pz) |
||
| 88 | { |
||
| 89 | float px2 = px * px; |
||
| 90 | float py2 = py * py; |
||
| 91 | float pz2 = pz * pz; |
||
| 92 | |||
| 93 | float pxz2 = px2 + pz2; |
||
| 94 | |||
| 95 | float alfa1,beta1,alfa2,beta2,gamma1; |
||
| 96 | float m,dsqrt; |
||
| 97 | |||
| 98 | float delta_xz = pxz2 - c0; |
||
| 99 | float s,k,k2,y1,delta_xy; |
||
| 100 | |||
| 101 | if (delta_xz < 0.0) return -1; |
||
| 102 | |||
| 103 | if (pz >= LEG_C) { |
||
| 104 | gamma1 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
||
| 105 | } else { |
||
| 106 | gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
||
| 107 | } |
||
| 108 | |||
| 109 | if (gamma1 < -90.0 || gamma1 > 90.0) return -1; |
||
| 110 | |||
| 111 | m = pxz2 - LEG_CD_2IPO * (sin(gamma1+LEG_CD_ANG)+cos(gamma1+LEG_CD_ANG)); |
||
| 112 | |||
| 113 | s = m + py2; |
||
| 114 | k = c2 + s; |
||
| 115 | k2 = k * k; |
||
| 116 | |||
| 117 | delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1); |
||
| 118 | |||
| 119 | if (delta_xy >= 0.0) { |
||
| 120 | dsqrt = sqrt(delta_xy); |
||
| 121 | y1 = (py * k + dsqrt) / (2.0 * s); |
||
| 122 | beta1 = asin(y1/LEG_B) * todeg; |
||
| 123 | alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1; |
||
| 124 | y1 = (py * k - dsqrt) / (2.0 * s); |
||
| 125 | beta2 = asin(y1/LEG_B) * todeg; |
||
| 126 | alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2; |
||
| 127 | |||
| 128 | if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) { |
||
| 129 | angles[n].a = (int)(alfa1 * 3600.0); |
||
| 130 | angles[n].b = (int)(beta1 * 3600.0); |
||
| 131 | angles[n].c = (int)(gamma1 * 3600.0); |
||
| 132 | #ifdef DEBUG_SEND |
||
| 133 | cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c); |
||
| 134 | #endif |
||
| 135 | return 0; |
||
| 136 | } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) { |
||
| 137 | angles[n].a = (int)(alfa2 * 3600.0); |
||
| 138 | angles[n].b = (int)(beta2 * 3600.0); |
||
| 139 | angles[n].c = (int)(gamma1 * 3600.0); |
||
| 140 | #ifdef DEBUG_SEND |
||
| 141 | cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c); |
||
| 142 | #endif |
||
| 143 | return 0; |
||
| 144 | } else { |
||
| 145 | #ifdef DEBUG_SEND |
||
| 146 | cprintf("No possible !\n"); |
||
| 147 | #endif |
||
| 148 | return 1; |
||
| 149 | } |
||
| 150 | } else |
||
| 151 | return 1; |
||
| 152 | |||
| 153 | return 0; |
||
| 154 | } |
||
| 155 | |||
| 156 | TASK servo_send() |
||
| 157 | { |
||
| 158 | HEXAPOD_STATE old_status; |
||
| 159 | register char changes, new_pos, new_pwm, new_power; |
||
| 160 | int n; |
||
| 161 | |||
| 162 | for (n=0; n<6;n++) { |
||
| 163 | old_status.leg[n].x = 150; |
||
| 164 | old_status.leg[n].y = 0; |
||
| 165 | old_status.leg[n].z = 0; |
||
| 166 | old_status.leg[n].pwm = 0; |
||
| 167 | } |
||
| 168 | old_status.power = 0; |
||
| 169 | |||
| 170 | while (1) { |
||
| 171 | changes = 0; |
||
| 172 | |||
| 173 | for (n=0; n<6; n++){ |
||
| 174 | new_pos = 0; |
||
| 175 | new_pwm = 0; |
||
| 176 | new_power = 0; |
||
| 177 | |||
| 178 | //sem_wait(&mx_status); |
||
| 179 | if ((status.leg[n].x != old_status.leg[n].x) || (status.leg[n].y != old_status.leg[n].y) || (status.leg[n].z != old_status.leg[n].z)) { |
||
| 180 | if (set_leg_position(n, status.leg[n].x, status.leg[n].y, status.leg[n].z)==0) { |
||
| 181 | old_status.leg[n].x = status.leg[n].x; |
||
| 182 | old_status.leg[n].y = status.leg[n].y; |
||
| 183 | old_status.leg[n].z = status.leg[n].z; |
||
| 184 | new_pos++; |
||
| 185 | } else { |
||
| 186 | #ifdef DEBUG_SEND |
||
| 187 | cprintf("Position not reachable.\n"); |
||
| 188 | #endif |
||
| 189 | status.leg[n].x = old_status.leg[n].x; |
||
| 190 | status.leg[n].y = old_status.leg[n].y; |
||
| 191 | status.leg[n].z = old_status.leg[n].z; |
||
| 192 | } |
||
| 193 | } |
||
| 194 | if (status.leg[n].pwm != old_status.leg[n].pwm) { |
||
| 195 | old_status.leg[n].pwm = status.leg[n].pwm; |
||
| 196 | new_pwm++; |
||
| 197 | } |
||
| 198 | if (status.power != old_status.power) { |
||
| 199 | old_status.power = status.power; |
||
| 200 | new_power++; |
||
| 201 | } |
||
| 202 | //sem_post(&mx_status); |
||
| 203 | |||
| 204 | if (new_pos) { |
||
| 205 | #ifdef SERIAL_ON |
||
| 206 | sem_wait(&mx_servo); |
||
| 1332 | giacomo | 207 | servo_set_angle_sec(COM_PORT, n*3+2, adjust(angles[n].a,n,0)); |
| 208 | servo_set_angle_sec(COM_PORT, n*3+1, adjust(angles[n].b,n,1)); |
||
| 209 | servo_set_angle_sec(COM_PORT, n*3 , adjust(angles[n].c,n,2)); |
||
| 1331 | giacomo | 210 | sem_post(&mx_servo); |
| 1332 | giacomo | 211 | |
| 212 | printf_xy(3,3,WHITE,"PX = %05d PY = %05d PZ = %05d",(int)(status.leg[n].x) |
||
| 213 | ,(int)(status.leg[n].y) |
||
| 214 | ,(int)(status.leg[n].z)); |
||
| 215 | |||
| 216 | printf_xy(3,4,WHITE,"ALFA = %07d BETA = %07d GAMMA = %07d",(int)(angles[n].a) |
||
| 217 | ,(int)(angles[n].b) |
||
| 218 | ,(int)(angles[n].c)); |
||
| 219 | |||
| 220 | printf_xy(3,5,WHITE,"ADJ_ALFA = %07d ADJ_BETA = %07d ADJ_GAMMA = %07d",adjust(angles[n].a,n,0),adjust(angles[n].b,n,1),adjust(angles[n].c,n,2)); |
||
| 221 | |||
| 1331 | giacomo | 222 | #endif |
| 223 | } |
||
| 224 | |||
| 225 | if (new_pwm) { |
||
| 226 | #ifdef SERIAL_ON |
||
| 227 | sem_wait(&mx_servo); |
||
| 228 | (old_status.leg[n].pwm & 1) ? servo_turn_on(COM_PORT, n*3+2) : servo_turn_off(COM_PORT, n*3+2); |
||
| 229 | (old_status.leg[n].pwm & 2) ? servo_turn_on(COM_PORT, n*3+1) : servo_turn_off(COM_PORT, n*3+1); |
||
| 230 | (old_status.leg[n].pwm & 4) ? servo_turn_on(COM_PORT, n*3 ) : servo_turn_off(COM_PORT, n*3 ); |
||
| 231 | sem_post(&mx_servo); |
||
| 232 | #endif |
||
| 233 | } |
||
| 234 | if (new_power) { |
||
| 235 | #ifdef SERIAL_ON |
||
| 236 | if (old_status.power) { |
||
| 237 | sem_wait(&mx_servo); |
||
| 238 | servo_set_RC5_switch(COM_PORT, 1); |
||
| 239 | sem_post(&mx_servo); |
||
| 240 | } else { |
||
| 241 | sem_wait(&mx_servo); |
||
| 242 | servo_set_RC5_switch(COM_PORT, 0); |
||
| 243 | sem_post(&mx_servo); |
||
| 244 | } |
||
| 245 | #endif |
||
| 246 | } |
||
| 247 | |||
| 248 | if (new_pos || new_pwm || new_power) { |
||
| 249 | changes++; |
||
| 250 | #ifdef DEBUG_SEND |
||
| 251 | print_status(n); |
||
| 252 | #endif |
||
| 253 | } |
||
| 254 | } |
||
| 255 | task_endcycle(); |
||
| 256 | } |
||
| 257 | |||
| 258 | return 0; |
||
| 259 | } |
||
| 260 | |||
| 261 | TASK servo_get() |
||
| 262 | { |
||
| 263 | int i = 0; |
||
| 264 | |||
| 265 | while (1) { |
||
| 266 | #ifdef SERIAL_ON |
||
| 267 | //sem_wait(&mx_status); |
||
| 268 | sem_wait(&mx_servo); |
||
| 269 | status.leg[i ].adc_in = servo_get_analog(COM_PORT, i ); |
||
| 270 | status.leg[i+1].adc_in = servo_get_analog(COM_PORT, i+1); |
||
| 271 | sem_post(&mx_servo); |
||
| 272 | #ifdef DEBUG_SEND |
||
| 273 | cprintf("Leg %1d-%1d: (%4d) (%4d)\n", i, i+1, status.leg[i].adc_in, status.leg[i+1].adc_in); |
||
| 274 | #endif |
||
| 275 | i = (i+2)%6; |
||
| 276 | //sem_post(&mx_status); |
||
| 277 | #endif |
||
| 278 | task_endcycle(); |
||
| 279 | } |
||
| 280 | |||
| 281 | return 0; |
||
| 282 | } |
||
| 283 | |||
| 284 | int init_serial() |
||
| 285 | { |
||
| 286 | int err; |
||
| 1332 | giacomo | 287 | err = servo_open(COM_PORT, 19200); |
| 1331 | giacomo | 288 | |
| 289 | return err; |
||
| 290 | } |
||
| 291 | |||
| 292 | void end_serial() |
||
| 293 | { |
||
| 294 | servo_close(COM_PORT); |
||
| 295 | } |
||
| 296 | |||
| 297 | void init_send_task() |
||
| 298 | { |
||
| 1332 | giacomo | 299 | SOFT_TASK_MODEL ms; |
| 1331 | giacomo | 300 | PID pid; |
| 301 | |||
| 1332 | giacomo | 302 | soft_task_default_model(ms); |
| 303 | soft_task_def_ctrl_jet(ms); |
||
| 304 | soft_task_def_met(ms, SEND_TASK_WCET); |
||
| 305 | soft_task_def_period(ms, SEND_TASK_MIT); |
||
| 306 | soft_task_def_usemath(ms); |
||
| 1331 | giacomo | 307 | pid = task_create("Send_Task", servo_send, &ms, NULL); |
| 308 | if (pid == NIL) { |
||
| 309 | perror("Could not create task <Send_Task>"); |
||
| 310 | sys_end(); |
||
| 311 | } else |
||
| 312 | task_activate(pid); |
||
| 313 | |||
| 1332 | giacomo | 314 | soft_task_default_model(ms); |
| 315 | soft_task_def_ctrl_jet(ms); |
||
| 316 | soft_task_def_met(ms, GET_TASK_WCET); |
||
| 317 | soft_task_def_period(ms, GET_TASK_MIT); |
||
| 318 | soft_task_def_usemath(ms); |
||
| 1331 | giacomo | 319 | pid = task_create("Get_Task", servo_get, &ms, NULL); |
| 320 | if (pid == NIL) { |
||
| 321 | perror("Could not create task <Get_Task>"); |
||
| 322 | sys_end(); |
||
| 323 | } else |
||
| 324 | task_activate(pid); |
||
| 325 | } |
||
| 326 | |||
| 327 | void init_send() |
||
| 328 | { |
||
| 329 | int i; |
||
| 330 | |||
| 331 | if (init_serial()) { |
||
| 332 | perror("Could not initialize serial port."); |
||
| 333 | sys_end(); |
||
| 334 | } |
||
| 335 | |||
| 336 | //sem_init(&mx_status,0,1); |
||
| 337 | sem_init(&mx_servo,0,1); |
||
| 338 | |||
| 339 | for (i=0; i<6;i++) { |
||
| 340 | status.leg[i].x = 150; |
||
| 341 | status.leg[i].y = 0; |
||
| 342 | status.leg[i].z = 0; |
||
| 343 | status.leg[i].pwm = 0; |
||
| 344 | } |
||
| 345 | status.power = 0; |
||
| 346 | |||
| 347 | init_send_task(); |
||
| 348 | } |
||
| 349 | |||
| 350 | void end_send() |
||
| 351 | { |
||
| 352 | end_serial(); |
||
| 353 | } |