void Sails::positionSails(int angle)
{
int servoAngle = 0;
int sailPos = 0;
int deltAngle = 0;
if (angle < 40)
{
servoAngle = SERVO_FULL_PULL_IN;
sails.writeMicroseconds(servoAngle);
}
else
{
// position of sails with [0 @ NO_GO_ZONE]
sailPos = map(angle, NO_GO_ZONE, 180, 0, NUMBER_OF_SAIL_POS);
// angle between consequetive sail positions
deltAngle = (abs(SERVO_FULL_TRIM - SERVO_FULL_PULL_IN)) / NUMBER_OF_SAIL_POS;
// servo angle for sails
servoAngle = SERVO_FULL_PULL_IN - deltAngle * sailPos;
// write angle to servo
sails.writeMicroseconds(servoAngle);
}
// Serial.print(test_angle);
Serial.print(", ");
Serial.print(angle); Serial.print(", ");
Serial.println(servoAngle);
}
// Spin motor at given power [-100, 100]
void rotateMotor(int pwr)
{
int val = constrain(pwr, -100, 100);
val = map(pwr, -100, 100, SERVO_MIN, SERVO_MAX);
#ifdef SERVO_WRITEUS
if(pwr == 0)
{
myservo.writeMicroseconds(SERVO_MIDPOINT);
}
else
{
myservo.writeMicroseconds(val);
}
#else
if(pwr == 0)
{
myservo.write(SERVO_MIDPOINT);
}
else
{
myservo.write(val);
}
#endif
}
void initMotors()
{
leftMotor.attach(LMPWMPin);
rightMotor.attach(RMPWMPin);
leftMotor.writeMicroseconds(1500); // set servo to mid-point
rightMotor.writeMicroseconds(1500);
}
void SetMotor(char dc, unsigned char mot)
{
if(mot==LEFT)
leftMotor.writeMicroseconds(1500+((int)dc)*5);
else
rightMotor.writeMicroseconds(1500+((int)dc)*5);
}
void Thruster_Stop()
{
motor1.writeMicroseconds(MID_SIGNAL);
motor2.writeMicroseconds(MID_SIGNAL);
motor3.writeMicroseconds(MID_SIGNAL);
motor4.writeMicroseconds(MID_SIGNAL);
}
int setSPD(int spd, unsigned long curTime)
{
/*
This function will set the speed of the servos. It takes spd argument in deg/sec (-300, 300), and uses an aproximated
calibration curve to set the pulse width. Sine the servos require some time to adjust the speed there is
lag check that will immedately return if there wasn't enough time between calls to the function
*/
unsigned long dur = curTime - spdTime;
if ( dur < SPD_WRITE_WAIT){ // return -1 if we haven't passed through the time limit, HOW do we prevent this from overflowing?
return -1;
}
spdTime = curTime; // Setting speed, so update clock
/*
This is an aproximated calibration curve, was built by hand using the servocalibrate degree sketch
*/
if ( spd > 300){
pos = 1700;
}
else if ( 270 < spd && spd <= 300){
pos = mapRange(spd, 270 , 300, 1600, 1700) + 0.5;
}
else if ( 180 < spd && spd <= 270) {
pos = mapRange(spd, 180, 270, 1550, 1600)+ 0.5;
}
else if ( 90 < spd && spd <= 180) {
pos = mapRange(spd, 90, 180, 1526, 1550)+ 0.5;
}
else if ( 1 <= spd && spd <= 90) {
pos = mapRange(spd, 1 , 90, 1501, 1526)+ 0.5;
}
else if ( 0 == spd) {
pos = 1500;
}
else if ( -90 < spd && spd <= -1) {
pos = mapRange(spd, -90 , -1, 1471, 1494)+ 0.5;
}
else if ( -180 < spd && spd <= -90) {
pos = mapRange(spd, -180 , -90, 1448, 1471)+ 0.5;
}
else if ( -270 < spd && spd <= -180) {
pos = mapRange(spd, -270 , -180, 1400, 1448)+ 0.5;
}
else if ( -300 <= spd && spd <= -270) {
pos = mapRange(spd, -300 , -270, 1300, 1400)+ 0.5;
}
else if (spd < -300 ) {
pos = 1300;
}
servo1.writeMicroseconds(pos); // Actully setting the speed.
rpos = mapRange(pos, 1300, 1700, 1700, 1300); //Reverse the driection for the other wheel
servo2.writeMicroseconds(rpos);
return pos; // otherwise return the pulse width value that came from the calibration curve.
}
void motorsInit( int leftPin, int rightPin )
{
servoLeft.attach(leftPin);
servoRight.attach(rightPin);
servoLeft.writeMicroseconds(servoSpeedLeft);
servoRight.writeMicroseconds(servoSpeedRight);
#ifdef USE_PID
Input = 0;
Setpoint = 0;
turningPID.SetMode(AUTOMATIC);
turningPID.SetOutputLimits(-SERVO_DRIVE_TURN_SPEED, SERVO_DRIVE_TURN_SPEED);
#endif
}
void Thruster_Speed(int *TH)
{
motor1.writeMicroseconds(TH[0]);
// Serial.println("1");
motor2.writeMicroseconds(TH[1]);
// Serial.println("2");
motor3.writeMicroseconds(TH[2]);
// Serial.println("3");
motor4.writeMicroseconds(TH[3]);
// Serial.println("4");
delay(TH[4]*1000);
}
void loop()
{
motor_VFL.writeMicroseconds(MotorPWM[MOTOR_VFL-1]);
motor_VFR.writeMicroseconds(MotorPWM[MOTOR_VFR-1]);
motor_VBL.writeMicroseconds(MotorPWM[MOTOR_VBL-1]);
motor_VBR.writeMicroseconds(MotorPWM[MOTOR_VBR-1]);
motor_HFL.writeMicroseconds(MotorPWM[MOTOR_HFL-1]);
motor_HFR.writeMicroseconds(MotorPWM[MOTOR_HFR-1]);
motor_HBL.writeMicroseconds(MotorPWM[MOTOR_HBL-1]);
motor_HBR.writeMicroseconds(MotorPWM[MOTOR_HBR-1]);
sDepth.read();
fDepth.data = -1.0 * sDepth.depth();//Mult by -1 so negative depth is down
pDepth.publish(&fDepth);
chatter.publish(&str_msg);
if (digitalRead(START_IN_PIN) == LOW){
bStart.data = true;
} else {
bStart.data = false;
}
if (digitalRead(STOP_IN_PIN) == LOW){
bStop.data = true;
} else {
bStop.data = false;
}
pStart.publish(&bStart);
pStop.publish(&bStop);
nh.spinOnce();
}
void speed(){
running = true; // make sure running is updated.
double temp_angle_diff = angle_diff * 180.0/3.14159;
temp_angle_diff = abs(temp_angle_diff);
if (temp_angle_diff < SPEED_TOGGLE_ANGLE){
esc.writeMicroseconds(S_HIGH);
telem_speed = S_HIGH;
}
else{
esc.writeMicroseconds(S_LOW);
telem_speed = S_LOW;
}
return ;
}
void motor_interrupt(void) {
int m = micros();
int dt = m - motor_prev_time;
if (dt>motor_min && dt<motor_max) {
motor_pwm_value = dt;
if(!lock) {
if (!cpu_lock) {
if (control_human) motor.writeMicroseconds(motor_pwm_value);
else motor.writeMicroseconds(cpu_motor_pwm_value);
}
}
if (cpu_lock) lock_stop();
}
motor_prev_time = m;
}
void servo_interrupt(void) {
int m = micros();
int dt = m-servo_prev_time;
if (dt>servo_min && dt<servo_max) {
servo_pwm_value = dt;
if (!lock) {
if (!cpu_lock) {
if (control_human) servo.writeMicroseconds(servo_pwm_value);
else servo.writeMicroseconds(cpu_servo_pwm_value);
}
}
if (cpu_lock) lock_stop();
}
servo_prev_time = m;
}
int Thruster_Init()
{
Serial.println("Initialize Thrusters ...");
motor1.attach(MOTOR1_PIN);
motor2.attach(MOTOR2_PIN);
motor3.attach(MOTOR3_PIN);
motor4.attach(MOTOR4_PIN);
motor1.writeMicroseconds(MID_SIGNAL);
motor2.writeMicroseconds(MID_SIGNAL);
motor3.writeMicroseconds(MID_SIGNAL);
motor4.writeMicroseconds(MID_SIGNAL);
delay(10000);
return 1;
}
void init()
{
controller.attach(pin);
goal = 1500;
cur = goal;
controller.writeMicroseconds(cur);
}
// Right => Positive
// Left => Left
void Sails::position(int pos)
{
// Center at 90deg
// Check for Range
sails.writeMicroseconds(pos);
}
void mode_test(){
SERIAL_OUT.println();
SERIAL_OUT.println();
SERIAL_OUT.println("toggle mode (i.e. TX CH3) and this function will print its current state");
SERIAL_OUT.println("perform hard reset to exit function");
esc.detach();
pinMode(THROTTLE, OUTPUT);
digitalWrite(THROTTLE, LOW);
bool attached = true;
long time_temp = 0;
while(1){
get_mode();
if(mode == MANUAL){
if(attached == true){
esc.detach();
// pinMode(THROTTLE, OUTPUT);
// digitalWrite(THROTTLE, LOW);
attached = false;
}
}
else if(mode == AUTOMATIC){
if(attached == false){
delay(250);
esc.attach(THROTTLE);
delay(250);
esc.writeMicroseconds(S_STOP);
attached = true;
}
}
else if(mode == AUX){
if(attached == true){
esc.detach();
// pinMode(THROTTLE, OUTPUT);
// digitalWrite(THROTTLE, LOW);
attached = false;
}
}
else if(mode == WP_MODE){
if(attached == true){
esc.detach();
// pinMode(THROTTLE, OUTPUT);
// digitalWrite(THROTTLE, LOW);
attached = false;
}
}
else SERIAL_OUT.println("nothing valid detected");
if((millis() - time_temp) > 250){
SERIAL_OUT.println(mode);
time_temp = millis();
}
}
return ;
}
void servo_test(){
steering.attach(STEERING);
steering.writeMicroseconds(STEER_ADJUST);
esc.attach(THROTTLE);
esc.write(90);
SERIAL_OUT.println();
SERIAL_OUT.println();
SERIAL_OUT.println("set CH3 to AUTOMATIC");
while(mode != AUTOMATIC) get_mode();
while(mode == AUTOMATIC){
SERIAL_OUT.println("normal angle output");
for(int pos = 30; pos < 150; pos += 1){ //goes from 0 degrees to 180 degrees in steps of 1 degree
steering.write(pos); //tell servo to go to position in variable 'pos'
SERIAL_OUT.println(pos);
delay(15); //waits 15ms for the servo to reach the position
}
for(int pos = 150; pos >= 30; pos -= 1){ //goes from 180 degrees to 0 degrees
steering.write(pos); //tell servo to go to position in variable 'pos'
SERIAL_OUT.println(pos);
delay(15); //waits 15ms for the servo to reach the position
}
SERIAL_OUT.println("microsecond angle output");
for(int pos = 1250; pos < 1750; pos += 1){ //goes from 0 degrees to 180 degrees in steps of 1 degree
steering.writeMicroseconds(pos); //tell servo to go to position in variable 'pos'
SERIAL_OUT.println(pos);
delay(5); //waits 15ms for the servo to reach the position
}
for(int pos = 1750; pos >= 1250; pos -= 1){ //goes from 180 degrees to 0 degrees
steering.writeMicroseconds(pos); //tell servo to go to position in variable 'pos'
SERIAL_OUT.println(pos);
delay(5); //waits 15ms for the servo to reach the position
}
get_mode();
}
esc.detach();
steering.detach();
return;
}
void steering_calibration(){
esc.detach();
SERIAL_OUT.println();
angle_target = 0.0;
steering.attach(STEERING);
steering.writeMicroseconds(STEER_ADJUST);
delay(500);
setup_mpu6050();
calculate_null();
SERIAL_OUT.println("set controller to automatic");
get_mode();
while(mode != AUTOMATIC) get_mode();
accum = 0; //this is ONLY used to reset the 0 the gyro angle for real (setting angle to 0 does nothing!!! (never forget last year's debacle))
delay(250);
esc.attach(THROTTLE);
delay(1000);
esc.writeMicroseconds(S_STOP);
delay(1000);
esc.writeMicroseconds(S_LOW);
while(mode == AUTOMATIC){
read_FIFO();
update_steering();
steering.writeMicroseconds(steer_us);
if((millis() - time) > 500){
SERIAL_OUT.print("angle: ");
SERIAL_OUT.print((angle*180.0/3.1415),5);
SERIAL_OUT.print("\tsteering ms: ");
SERIAL_OUT.println(steer_us);
time = millis();
}
get_mode();
}
steering.detach();
esc.detach();
return ;
}
void loop() {
int c = status % 4;
if(0 == c){
myservo.writeMicroseconds(MIN_POS);
Serial.println("forward");
}
else if(2 == c){
myservo.writeMicroseconds(MAX_POS);
Serial.println("backward");
}
else{
myservo.writeMicroseconds(CENTER_POS);
Serial.println("stop");
}
status++;
delay(2000);
}
void CameraMount::device_loop(Command command){
if (command.cmp("tilt")) {
tilt_val = command.args[1];
}
if (tilt_val != new_tilt){
new_tilt = smoothAdjustedCameraPosition(tilt_val,new_tilt);
tilt.writeMicroseconds(new_tilt);
}
}
void button_interrupt(void) {
int m = micros();
int dt = m-servo_prev_time;
if (dt>0.8*button_min && dt<1.2*button_max) {
button_pwm_value = dt;
if (abs(button_pwm_value-bottom_button)<50) {
lock = 1;
motor.writeMicroseconds(motor_null);
servo.writeMicroseconds(servo_null);
digitalWrite(ledPin, HIGH);
}
if (abs(button_pwm_value-top_button)<50) {
lock = 0;
digitalWrite(ledPin, LOW);
}
}
button_prev_time = m;
}
void InitESCCallback(const InitESC::Request & req, InitESC::Response & res){
motor_VFL.writeMicroseconds(STOP_PWM);
motor_VFR.writeMicroseconds(STOP_PWM);
motor_VBL.writeMicroseconds(STOP_PWM);
motor_VBR.writeMicroseconds(STOP_PWM);
motor_HFL.writeMicroseconds(STOP_PWM);
motor_HFR.writeMicroseconds(STOP_PWM);
motor_HBL.writeMicroseconds(STOP_PWM);
motor_HBR.writeMicroseconds(STOP_PWM);
delay(1000);
}
//sets up variables and starts timed PID routine
void initPID(void)
{
unsigned char i;
//PID Setup
for(i=0;i<NUM_PID;i++)
{
SumError[i]=0;
TargetRPM[i]=0;
stallCt[i]=0;
}
if(PID_ON)
pidTimerSetup();
else
Serial.println("PID IS OFF!");
leftMotor.attach(LMPWMPin);
rightMotor.attach(RMPWMPin);
leftMotor.writeMicroseconds(1500); // set servo to mid-point
rightMotor.writeMicroseconds(1500);
}
int check_for_error_conditions(void) {
// Check state of all of these variables for out-of-bound conditions
// If in calibration state, ignore potential errors in order to attempt to correct.
if (state == STATE_LOCK_CALIBRATE) return(1);
if (
safe_pwm_range(servo_null_pwm_value) &&
safe_pwm_range(servo_max_pwm_value) &&
safe_pwm_range(servo_min_pwm_value) &&
safe_pwm_range(motor_null_pwm_value) &&
safe_pwm_range(motor_max_pwm_value) &&
safe_pwm_range(motor_min_pwm_value) &&
safe_pwm_range(servo_pwm_value) &&
safe_pwm_range(motor_pwm_value) &&
safe_pwm_range(button_pwm_value) &&
button_prev_interrupt_time >= 0 &&
servo_prev_interrupt_time >= 0 &&
motor_prev_interrupt_time >= 0 &&
state_transition_time_ms >= 0 &&
safe_percent_range(caffe_servo_percent) &&
safe_percent_range(caffe_motor_percent) &&
safe_percent_range(servo_percent) &&
safe_percent_range(motor_percent) &&
safe_pwm_range(caffe_servo_pwm_value) &&
safe_pwm_range(caffe_motor_pwm_value) &&
caffe_last_int_read_time >= 0 &&
caffe_mode >= -3 && caffe_mode <= 9 &&
state >= -1 && state <= 100 &&
previous_state >= -1 && previous_state <= 100
) return(1);
else {
if (state != STATE_ERROR) {
// On first entering error state, attempt to null steering and motor
servo_pwm_value = servo_null_pwm_value;
motor_pwm_value = motor_null_pwm_value;
servo.writeMicroseconds(servo_null_pwm_value);
motor.writeMicroseconds(motor_null_pwm_value);
}
state = STATE_ERROR;
return(0);
}
}
void servo_interrupt(void) {
int m = micros();
int dt = m-servo_prev_time;
if (dt>0.8*servo_min && dt<1.2*servo_max) {
servo_pwm_value = dt;
if (!lock) {
servo.writeMicroseconds(servo_pwm_value);
}
}
servo_prev_time = m;
}
void motor_interrupt(void) {
int m = micros();
int dt = m - motor_prev_time;
if (dt>0.8*motor_min && dt<1.2*motor_max) {
motor_pwm_value = dt;
if(!lock) {
motor.writeMicroseconds(motor_pwm_value);
}
}
motor_prev_time = m;
}
//sweeps the motor speed from reset to 0, go to 100%, reset to 0, go to -100%
void sweepSpeed()
{
Serial.println("Sweeping Speeds 0 to 100, then 0 to -100");
for(int i=0;i<=5;i++)
{
leftMotor.writeMicroseconds(1000+((i+5)%11)*100);
Serial.println(1500+i*100);
delay(500);
toggleTestPin();
}
leftMotor.writeMicroseconds(1500); //stop
Serial.println(1500);
for(int i=0;i<=10;i++)
{
leftMotor.writeMicroseconds(1000+((i+5)%11)*100);
Serial.println(2000+i*100);
delay(500);
toggleTestPin();
}
}
//
////////////////////////////////////////
////////////////////////////////////////
//
// Servo interrupt service routine. This would be very short except that the human can take
// control from Caffe, and Caffe can take back control if steering left in neutral position.
void servo_interrupt_service_routine(void) {
volatile long int m = micros();
volatile long int dt = m - servo_prev_interrupt_time;
servo_prev_interrupt_time = m;
if (dt < 0) return; // this will occur when micros rolls over
if (state == STATE_ERROR) return; // no action if in error state
if (dt>SERVO_MIN && dt<SERVO_MAX) {
servo_pwm_value = dt;
if (state == STATE_HUMAN_FULL_CONTROL) {
servo.writeMicroseconds(servo_pwm_value);
}
else if (state == STATE_CAFFE_HUMAN_STEER_HUMAN_MOTOR) {
// If steer is close to neutral, let Caffe take over.
if (abs(servo_pwm_value-servo_null_pwm_value)<=30 ){
previous_state = state;
state = STATE_CAFFE_CAFFE_STEER_HUMAN_MOTOR;
state_transition_time_ms = m/1000.0;
//servo.writeMicroseconds((caffe_servo_pwm_value+servo_pwm_value)/2);
}
else {
servo.writeMicroseconds(servo_pwm_value);
}
}
// If human makes strong steer command, let human take over.
else if (state == STATE_CAFFE_CAFFE_STEER_HUMAN_MOTOR) {
if (abs(servo_pwm_value-servo_null_pwm_value)>70) {
previous_state = state;
state = STATE_CAFFE_HUMAN_STEER_HUMAN_MOTOR;
state_transition_time_ms = m/1000.0;
//servo.writeMicroseconds(servo_pwm_value);
}
else {
servo.writeMicroseconds(caffe_servo_pwm_value);
}
}
else {
;//servo.writeMicroseconds(servo_null_pwm_value);
}
}
}
//
////////////////////////////////////////
////////////////////////////////////////
//
// Motor interrupt service routine. This is simple because right now only human controls motor.
void motor_interrupt_service_routine(void) {
volatile long int m = micros();
volatile long int dt = m - motor_prev_interrupt_time;
motor_prev_interrupt_time = m;
// Locking out in error state has bad results -- cannot switch off motor manually if it is on.
// if (state == STATE_ERROR) return;
if (dt>MOTOR_MIN && dt<MOTOR_MAX) {
motor_pwm_value = dt;
if (state == STATE_HUMAN_FULL_CONTROL) {
motor.writeMicroseconds(motor_pwm_value);
}
else if (state == STATE_CAFFE_HUMAN_STEER_HUMAN_MOTOR) {
motor.writeMicroseconds(motor_pwm_value);
}
else if (state == STATE_CAFFE_CAFFE_STEER_HUMAN_MOTOR) {
motor.writeMicroseconds(motor_pwm_value);
}
else {
;//motor.writeMicroseconds(motor_null_pwm_value);
}
}
}
//Should be called in each loop so PWM slowly ramps up, doesn't work otherwise
void run()
{
if (cur != goal)
{
if (goal == 1500) cur = 1500;
else if (goal < 1500)
{
cur -= 100;
}
else if (goal > 1500)
{
cur += 100;
}
controller.writeMicroseconds(cur);
}
}
