Actuators.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "Config.h"
00004 
00005 extern Config config;
00006 
00007 Servo steering(p21);                    // Steering Servo
00008 Servo throttle(p22);                    // ESC
00009 int escMax=520;                         // Servo setting for max speed
00010 
00011 #define THROTTLE_CENTER 0.4
00012 
00013 void initSteering()
00014 {
00015     // Setup steering servo
00016     steering = config.steerZero;
00017     // TODO: 3 parameterize this in config file
00018     steering.calibrate(0.005, 45.0); 
00019 }
00020 
00021 
00022 void initThrottle()
00023 {
00024     throttle = (float)config.escZero/1000.0;
00025     // TODO: 3 parameterize this in config file
00026     throttle.calibrate(0.001, 45.0); 
00027 }
00028 
00029 void setThrottle(int value)
00030 {
00031     throttle = ((float)value)/1000.0;
00032 }
00033 
00034 void setSteering(float steerAngle)
00035 {
00036     // Convert steerAngle to servo value
00037     // Testing determined near linear conversion between servo ms setting and steering angle
00038     // up to 20*.  Assumes a particular servo library with range = 0.005
00039     // In that case, f(SA) = servoPosition = 0.500 + SA/762.5
00040     // between 20 and 24* the slope is approximately 475
00041     // What if we ignore the linearity and just set to a max angle
00042     // also range is 0.535-0.460 --> slope = 800
00043     // steering = 0.500 + (double) steerAngle / 762.5;
00044     //
00045     // TODO: 1 parameterize through config
00046     steering = 0.500 + (double) steerAngle / 808.0;
00047 }
00048