X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Frobot_system%2Frobot_system.c;h=b5b3d32824597e3e7af16f7b2dae0d59df4625bd;hp=5e91561372e6471dc97604ef4011f224cf5bd695;hb=b14123cac428083a50e2d0871514018810a779e5;hpb=48ea32b5f2e7e3eba76955e458066eac050ce965 diff --git a/modules/devices/robot/robot_system/robot_system.c b/modules/devices/robot/robot_system/robot_system.c index 5e91561..b5b3d32 100755 --- a/modules/devices/robot/robot_system/robot_system.c +++ b/modules/devices/robot/robot_system/robot_system.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -40,7 +40,7 @@ #include "robot_system.h" -/** Call a pwm() pointer : +/** Call a pwm() pointer : * - lock the interrupts * - read the pointer to the pwm function * - unlock the interrupts @@ -62,7 +62,7 @@ safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value) } } -/** Call a encoder() pointer : +/** Call a encoder() pointer : * - lock the interrupts * - read the pointer to the encoder function * - unlock the interrupts @@ -134,7 +134,7 @@ void rs_set_right_pwm(struct robot_system * rs, void (*right_pwm)(void *, int32_ #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT /** define left motor encoder function and param */ -void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *), +void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *), void *left_mot_encoder_param, double gain) { uint8_t flags; @@ -147,7 +147,7 @@ void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encode } /** define right motor encoder function and param */ -void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *), +void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *), void *right_mot_encoder_param, double gain) { uint8_t flags; @@ -161,7 +161,7 @@ void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_enco #endif /** define left external encoder function and param */ -void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *), +void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *), void *left_ext_encoder_param, double gain) { uint8_t flags; @@ -178,7 +178,7 @@ void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encode } /** define right external encoder function and param */ -void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *), +void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *), void *right_ext_encoder_param, double gain) { uint8_t flags; @@ -196,9 +196,9 @@ void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_enco /**** Virtual encoders and PWM */ -/** +/** * set the real pwms according to the specified angle (it also - * depends on the last distance command sent) + * depends on the last distance command sent) */ void rs_set_angle(void * data, int32_t angle) { @@ -214,14 +214,14 @@ void rs_set_angle(void * data, int32_t angle) p.angle = angle; rs_get_wheels_from_polar(&w, &p); - + safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left); safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right); } -/** +/** * set the real pwms according to the specified distance (it also - * depends on the last angle command sent) + * depends on the last angle command sent) */ void rs_set_distance(void * data, int32_t distance) { @@ -237,37 +237,37 @@ void rs_set_distance(void * data, int32_t distance) p.distance = distance; rs_get_wheels_from_polar(&w, &p); - + safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left); safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right); } -/** - * get the virtual angle according to real encoders value. +/** + * get the virtual angle according to real encoders value. */ int32_t rs_get_angle(void * data) { struct robot_system * rs = data; int32_t angle; uint8_t flags; - + IRQ_LOCK(flags); - angle = rs->virtual_encoders.angle ; + angle = rs->virtual_encoders.angle ; IRQ_UNLOCK(flags); return angle; } -/** - * get the virtual distance according to real encoders value. +/** + * get the virtual distance according to real encoders value. */ int32_t rs_get_distance(void * data) { struct robot_system * rs = data; int32_t distance; uint8_t flags; - + IRQ_LOCK(flags); - distance = rs->virtual_encoders.distance ; + distance = rs->virtual_encoders.distance ; IRQ_UNLOCK(flags); return distance; } @@ -277,9 +277,9 @@ int32_t rs_get_ext_angle(void * data) struct robot_system * rs = data; int32_t angle; uint8_t flags; - + IRQ_LOCK(flags); - angle = rs->pext_prev.angle ; + angle = rs->pext_prev.angle ; IRQ_UNLOCK(flags); return angle; } @@ -289,9 +289,9 @@ int32_t rs_get_ext_distance(void * data) struct robot_system * rs = data; int32_t distance; uint8_t flags; - + IRQ_LOCK(flags); - distance = rs->pext_prev.distance ; + distance = rs->pext_prev.distance ; IRQ_UNLOCK(flags); return distance; } @@ -301,10 +301,10 @@ int32_t rs_get_mot_angle(void * data) { struct robot_system * rs = data; int32_t angle; - uint8_t flags; + uint8_t flags; IRQ_LOCK(flags); - angle = rs->pmot_prev.angle ; + angle = rs->pmot_prev.angle ; IRQ_UNLOCK(flags); return angle; } @@ -314,9 +314,9 @@ int32_t rs_get_mot_distance(void * data) struct robot_system * rs = data; int32_t distance; uint8_t flags; - + IRQ_LOCK(flags); - distance = rs->pmot_prev.distance ; + distance = rs->pmot_prev.distance ; IRQ_UNLOCK(flags); return distance; } @@ -327,9 +327,9 @@ int32_t rs_get_ext_left(void * data) struct robot_system * rs = data; int32_t left; uint8_t flags; - + IRQ_LOCK(flags); - left = rs->wext_prev.left ; + left = rs->wext_prev.left ; IRQ_UNLOCK(flags); return left; } @@ -339,9 +339,9 @@ int32_t rs_get_ext_right(void * data) struct robot_system * rs = data; int32_t right; uint8_t flags; - + IRQ_LOCK(flags); - right = rs->wext_prev.right ; + right = rs->wext_prev.right ; IRQ_UNLOCK(flags); return right; } @@ -351,10 +351,10 @@ int32_t rs_get_mot_left(void * data) { struct robot_system * rs = data; int32_t left; - uint8_t flags; + uint8_t flags; IRQ_LOCK(flags); - left = rs->wmot_prev.left ; + left = rs->wmot_prev.left ; IRQ_UNLOCK(flags); return left; } @@ -364,9 +364,9 @@ int32_t rs_get_mot_right(void * data) struct robot_system * rs = data; int32_t right; uint8_t flags; - + IRQ_LOCK(flags); - right = rs->wmot_prev.right ; + right = rs->wmot_prev.right ; IRQ_UNLOCK(flags); return right; } @@ -375,13 +375,13 @@ int32_t rs_get_mot_right(void * data) void rs_set_flags(struct robot_system * rs, uint8_t flags) { uint8_t i_flags; - + IRQ_LOCK(i_flags); rs->flags = flags; IRQ_UNLOCK(i_flags); } -/** +/** * Read the encoders, and update internal virtual counters. Call this * function is needed before reading the virtual encoders.The program * will decide if it the external encoders or the motor encoders are @@ -398,7 +398,7 @@ void rs_update(void * data) #endif int32_t delta_angle, delta_distance; uint8_t flags; - + /* read encoders */ wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param); wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param); @@ -407,7 +407,7 @@ void rs_update(void * data) wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param); wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param); #endif - + /* apply gains to each wheel */ if (! (rs->flags & RS_IGNORE_EXT_GAIN )) { #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64