amtec_base.c
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 #include "amtec_base.h"
00038 
00040 #define D2R       0.0174532925199432957692
00041 
00042 #define R2D       57.295779513082320876
00043 
00044 #define AMTEC_DEFAULT_PAN_ID          14
00045 #define AMTEC_DEFAULT_PAN_MIN_POS   -200.0
00046 #define AMTEC_DEFAULT_PAN_MAX_POS    200.0
00047 #define AMTEC_DEFAULT_PAN_MAX_VEL    0.5
00048 #define AMTEC_DEFAULT_PAN_MAX_ACC    0.4
00049 #define AMTEC_DEFAULT_PAN_MAX_CUR    4.0
00050 #define AMTEC_DEFAULT_PAN_TARGET_VEL 0.5
00051 #define AMTEC_DEFAULT_PAN_TARGET_ACC 0.4
00052 
00053 #define AMTEC_DEFAULT_TILT_ID         13
00054 #define AMTEC_DEFAULT_TILT_MIN_POS   -90.0
00055 #define AMTEC_DEFAULT_TILT_MAX_POS    90.0
00056 #define AMTEC_DEFAULT_TILT_MAX_VEL    0.3
00057 #define AMTEC_DEFAULT_TILT_MAX_ACC    0.2
00058 #define AMTEC_DEFAULT_TILT_MAX_CUR    4.0
00059 #define AMTEC_DEFAULT_TILT_TARGET_VEL 0.3
00060 #define AMTEC_DEFAULT_TILT_TARGET_ACC 0.2
00061 
00062 
00063 #define AMTEC_DEFAULT_PAN_ACC          0.4
00064 #define AMTEC_DEFAULT_PAN_VEL          0.5
00065 
00066 #define AMTEC_DEFAULT_TILT_ACC         0.2
00067 #define AMTEC_DEFAULT_TILT_VEL         0.3
00068 
00069 void amtecInitializeDevice(amtec_powercube_device_p p)
00070 {
00071   strncpy(p->ttyport, "/dev/ttyUSB3", MAX_NAME_LENGTH);
00072   p->baud = 38400;
00073   p->parity = N;
00074   p->fd = -1;
00075   p->databits = 8;
00076   p->stopbits = 1;
00077   p->hwf = 0;
00078   p->swf = 0;
00079 }
00080 
00081 void amtecInitializePanSettings(amtec_powercube_setting_p p)
00082 {
00083   p->MinPos = AMTEC_DEFAULT_PAN_MIN_POS;
00084   p->MaxPos = AMTEC_DEFAULT_PAN_MAX_POS;
00085   p->MaxVel = AMTEC_DEFAULT_PAN_MAX_VEL;
00086   p->MaxAcc = AMTEC_DEFAULT_PAN_MAX_ACC;
00087   p->MaxCur = AMTEC_DEFAULT_PAN_MAX_CUR;
00088   p->TargetVel = AMTEC_DEFAULT_PAN_TARGET_VEL;
00089   p->TargetAcc = AMTEC_DEFAULT_PAN_TARGET_ACC;
00090 }
00091 
00092 void amtecInitializeTiltSettings(amtec_powercube_setting_p p)
00093 {
00094   p->MinPos = AMTEC_DEFAULT_TILT_MIN_POS;
00095   p->MaxPos = AMTEC_DEFAULT_TILT_MAX_POS;
00096   p->MaxVel = AMTEC_DEFAULT_TILT_MAX_VEL;
00097   p->MaxAcc = AMTEC_DEFAULT_TILT_MAX_ACC;
00098   p->MaxCur = AMTEC_DEFAULT_TILT_MAX_CUR;
00099   p->TargetVel = AMTEC_DEFAULT_TILT_TARGET_VEL;
00100   p->TargetAcc = AMTEC_DEFAULT_TILT_TARGET_ACC;
00101 }
00102 
00103 void amtecInitializeParams(amtec_powercube_params_p p)
00104 {
00105   p->id = -1;
00106   p->DefHomeOffset = -1;
00107   p->DefMinPos = -1;
00108   p->DefMaxPos = -1;
00109   p->DefMaxVel = -1;
00110   p->DefMaxAcc = -1;
00111   p->DefMaxCur = -1;
00112   p->DefCubeSerial = -1;
00113   p->DefConfig = -1;
00114   p->DefCubeVersion = -1;
00115   p->DefScndBaud = -1;
00116   p->ActPos = -1;
00117   p->ActVel = -1;
00118   p->MinPos = -1;
00119   p->MaxPos = -1;
00120   p->MaxVel = -1;
00121   p->MaxAcc = -1;
00122   p->MaxCur = -1;
00123   p->Config = -1;
00124   p->UseBreak = -1;
00125 
00126   p->ActC0 = -1;
00127   p->ActDamp = -1;
00128   p->ActA0 = -1;
00129 }
00130 
00131 void amtecInitializeSettings(amtec_powercube_p powercube)
00132 {
00133   amtecInitializeParams(&(powercube->pan));
00134   amtecInitializePanSettings(&(powercube->panset));
00135   powercube->pan.id = AMTEC_DEFAULT_PAN_ID;
00136 
00137   amtecInitializeParams(&(powercube->tilt));
00138   amtecInitializeTiltSettings(&(powercube->tiltset));
00139   powercube->tilt.id = AMTEC_DEFAULT_TILT_ID;
00140 
00141   amtecInitializeDevice(&(powercube->dev));
00142 }
00143 
00144 amtec_powercube_p amtecInitialize()
00145 {
00146   amtec_powercube_p powercube;
00147   powercube = (amtec_powercube_p)calloc(1, sizeof(amtec_powercube_t));
00148   amtecInitializeSettings(powercube);
00149   return powercube;
00150 }
00151 
00152 void amtecClear(amtec_powercube_p powercube)
00153 {
00154   free(powercube);
00155 }
00156 
00157 void amtecPrintParams(amtec_powercube_params_p params)
00158 {
00159   fprintf(stderr, "DefHomeOffset  = %4.3f\n", R2D*(params->DefHomeOffset));
00160   fprintf(stderr, "DefGearRatio   = %4.3f\n", params->DefGearRatio);
00161   fprintf(stderr, "DefLinRatio    = %4.3f\n", params->DefLinRatio);
00162   fprintf(stderr, "DefMinPos      = %4.3f\n", R2D*(params->DefMinPos));
00163   fprintf(stderr, "DefMaxPos      = %4.3f\n", R2D*(params->DefMaxPos));
00164   fprintf(stderr, "DefMaxDeltaPos = %4.3f\n", R2D*(params->DefMaxDeltaPos));
00165 //  fprintf(stderr, "DefMaxDeltaVel = %4.3f\n", params->DefMaxDeltaVel);
00166   fprintf(stderr, "DefTorqueRatio = %4.3f\n", params->DefTorqueRatio);
00167   fprintf(stderr, "DefCurRatio    = %4.3f\n", params->DefCurRatio);
00168   fprintf(stderr, "DefMaxVel      = %4.3f\n", params->DefMaxVel);
00169   fprintf(stderr, "DefMaxAcc      = %4.3f\n", params->DefMaxAcc);
00170   fprintf(stderr, "DefMaxCur      = %4.3f\n", params->DefMaxCur);
00171   fprintf(stderr, "DefHomeVel     = %4.3f\n", params->DefHomeVel);
00172   fprintf(stderr, "DefHomeAcc     = %4.3f\n", params->DefHomeAcc);
00173 
00174   fprintf(stderr, "DefCubeSerial  = %u\n", params->DefCubeSerial);
00175   fprintf(stderr, "DefConfig      = %u\n", params->DefConfig);
00176   fprintf(stderr, "DefPulsesPerTurn = %u\n", params->DefPulsesPerTurn);
00177   fprintf(stderr, "DefCubeVersion = %u\n", params->DefCubeVersion);
00178   fprintf(stderr, "DefServiceInterval = %u\n", params->DefServiceInterval);
00179   fprintf(stderr, "DefBrakeTimeOut= %u\n", params->DefBrakeTimeOut);
00180   fprintf(stderr, "DefAddress     = %u\n", params->DefAddress);
00181   fprintf(stderr, "DefPrimBaud    = %u\n", params->DefPrimBaud);
00182   fprintf(stderr, "DefScndBaud    = %u\n", params->DefScndBaud);
00183   fprintf(stderr, "PosCount       = %d\n", params->PosCount);
00184   fprintf(stderr, "RefPosCount    = %d\n", params->RefPosCount);
00185   fprintf(stderr, "DioSetup       = %u\n", params->DioSetup);
00186   fprintf(stderr, "CubeState      = %u\n", params->CubeState);
00187   fprintf(stderr, "TargetPosInc   = %u\n", params->TargetPosInc);
00188   fprintf(stderr, "TargetVelInc   = %u\n", params->TargetVelInc);
00189   fprintf(stderr, "TargetAccInc   = %u\n", params->TargetAccInc);
00190   fprintf(stderr, "StepInc        = %u\n", params->StepInc);
00191 
00192 
00193   // fprintf(stderr, "  DefSendBaud    = %u\n", params->DefSendBaud);
00194   fprintf(stderr, "MinPos         = %4.3f\n", R2D*(params->MinPos));
00195   fprintf(stderr, "MaxPos         = %4.3f\n", R2D*(params->MaxPos));
00196   fprintf(stderr, "MaxVel         = %4.3f\n", params->MaxVel);
00197   fprintf(stderr, "MaxAcc         = %4.3f\n", params->MaxAcc);
00198   fprintf(stderr, "MaxCur         = %4.3f\n", params->MaxCur);
00199   // fprintf(stderr, "  TargetAcc      = %4.3f\n", params->TargetAcc);
00200   // fprintf(stderr, "  TargetVel      = %4.3f\n", params->TargetVel);
00201   fprintf(stderr, "ActPos         = %4.3f\n", R2D*(params->ActPos));
00202   fprintf(stderr, "ActVel         = %4.3f\n", params->ActVel);
00203   fprintf(stderr, "Break          = %d\n", (int) ((params->Config >> 3) & 1));
00204 
00205   fprintf(stderr, "ActC0          = %d\n", params->ActC0);
00206   fprintf(stderr, "ActDamp        = %d\n", params->ActDamp);
00207   fprintf(stderr, "ActA0          = %d\n", params->ActA0);
00208 }
00209 
00210 void amtecPrintInformation(amtec_powercube_p powercube) {
00211   fprintf(stderr, "\n------ PAN -----\n");
00212   amtecPrintParams(&powercube->pan);
00213   fprintf(stderr, "\n");
00214   fprintf(stderr, "\n------ TILT -----\n");
00215   amtecPrintParams(&powercube->tilt);
00216 }
00217 


amtec
Author(s): Charles DuHadway, Benjamin Pitzer (Maintained by Benjamin Pitzer)
autogenerated on Mon Oct 6 2014 10:10:36