Robot Simulator of the Robotics Group for Self-Organization of Control: spring.h Source File

# spring.h

Go to the documentation of this file.

    00001 /*
    00002  * spring.h
    00003  *
    00004  * This is a OneAxisServo that leaves the position coordinate unscaled
    00005  * and unshifted. The 0 position is that of initialization of the two joint
    00006  * parts. It now allows preloading of the spring and more control but also
    00007  * more errors. Use carefully!
    00008  *
    00009  *  Created on: Nov 2, 2010
    00010  *      Author: mab
    00011  */
    00012
    00013 #ifndef SPRING_H_
    00014 #define SPRING_H_
    00015
    00016
    00017 #include <ode_robots/joint.h>
    00018 #include <ode_robots/oneaxisservo.h>
    00019 #include <ode_robots/pid.h>
    00020 #include <ode_robots/angularmotor.h>
    00021 #include <selforg/controller_misc.h>
    00022
    00023 namespace lpzrobots {
    00024
    00025 /* This is a OneAxisServo that leaves the position coordinate unscaled
    00026 * and unshifted. The 0 position is that of initialization of the two joint
    00027 * parts. It now allows preloading of the spring and more control but also
    00028 * more errors. Use carefully!
    00029 */
    00030 class Spring : public OneAxisServo{
    00031 public:
    00032         Spring(OneAxisJoint* joint, double _min, double _max,
    00033                          double power, double damp=0.2, double integration=0.0,
    00034                          double maxVel=10.0, double jointLimit = 1.0)
    00035      : OneAxisServo(joint, _min, _max, power, damp, integration, maxVel, jointLimit, false){
    00036         }
    00037         virtual ~Spring(){}
    00038
    00039     /** sets the set point of the servo.
    00040         Position is relative to initial position of the two parts
    00041         connected by the spring
    00042     */
    00043     virtual void set(double pos){
    00044         pid.setTargetPosition(pos);
    00045
    00046         double force = pid.stepNoCutoff(joint->getPosition1(), joint->odeHandle.getTime());
    00047         force = clip(force,-10*pid.KP, 10*pid.KP); // limit force to 10*KP
    00048         joint->addForce1(force);
    00049         if(maxVel>0){
    00050             joint->getPart1()->limitLinearVel(maxVel);
    00051             joint->getPart2()->limitLinearVel(maxVel);
    00052         }
    00053     }
    00054
    00055     /** returns the position of the slider in ranges [-1, 1] (scaled by min, max)*/
    00056     virtual double get(){
    00057         double pos =  joint->getPosition1();
    00058         return pos;
    00059     }
    00060
    00061     //want to allow all kinds of borders,
    00062     virtual void setMinMax(double _min, double _max){
    00063           min=_min;
    00064           max=_max;
    00065           joint->setParam(dParamLoStop, min - fabs(min) * (jointLimit-1));
    00066           joint->setParam(dParamHiStop, max + fabs(max) * (jointLimit-1));
    00067         }
    00068
    00069 };
    00070
    00071 }
    00072
    00073 #endif /* SPRING_H_ */