I'm trying to control a servo motor using a speed control. I am mapping from zero to 95 degrees and back. The issue I'm encountering is that in the fourth segment of the motion the if statement fails once the servo reaches 44 degrees. If someone could help me understand why this is happening that would be great. This code can be run with no servo, the angle is printed.
The code is here:
#include <math.h>
#include <Servo.h>
//this one measures the translational speed of the test rig and rotates the hip joint at a
//corresponding rate: This code does not use an interrupt to measure speed. Rather the
//photosensor is sampled once per loop cycle. This is done under the assumption that the wheel
//will not be spinning too fast for the individual clicks to be sensed. This code has been further
//modified to select the segment of the motion based on an integrated angle value rather than the
//number of clicks since the program began. Hope.
//Make servo objects:
Servo servo1;
Servo servo2;
Servo servo3;
//declare the variables used to run the spedometer:
const byte phot_sens_pin = 2;
bool a_phase_init;
bool a_phase_new;
bool a_phase_prev;
//declare the variables used to calculate the speed and to move the leg:
long time_int = 1000000;
unsigned long prev_time = 0;
unsigned long new_time = 0;
double walk_speed = 0;
double prev_speed = 0;
const double stride_len = 1; //m
const double wheel_circ = PI*.1; //m
const int num_seg = 8;
const double wheel_circ_seg = (wheel_circ/num_seg)*1000000; //m
double move_times[4];
unsigned long move_start_time = 0;
unsigned long progress = 0;
long motion_seg = 0;
void reset_clock();
bool fourth_seg = false;
//Establish lengths of joints and stride:
const double Lo2 = 1; //Stride Length, m
const double Lthigh = 0.4572; //thigh length, m
const double Lshin = 0.4572; //shin length, m
const double Lfoot = 0.2032; //foot length, m
//Determine range of angles the hip, knee, and ankle will move through:
double thigh_angle_calc = Lo2/(Lthigh+Lshin);
const double THL = 2* (atan(thigh_angle_calc) * 180/3.14159265); //Lower bound of thigh angle range
const double THU = atan(thigh_angle_calc) * 180/3.14159265; //Upper bound of thigh angle range
const double RH = THL; //Magnitude of thigh angle range
//const double RH_1 = THL/4.;
//const double RH_2 = THL/2.;
//const double RH_3 = THL;
double AH; //Variable angle of the hip
double hip_angles[4][2] = {{0, 0.25*RH},
{0.25*RH, 0.5*RH },
{0.5*RH, RH },
{RH, 0 },};
int step_det;
//Set up servo pins:
const int hip_servo = 3;
const int knee_servo = 5;
const int ankle_servo = 6;
//provide values for the ankle and knee angles, these will be held constant during the motion:
double AA = 45;
double AK = 0;
int counter = 0;
double correct_count = 0;
double fuck_up_count = 0;
void setup() {
//attach the servo pin:
servo1.attach(hip_servo);
servo2.attach(knee_servo);
servo3.attach(ankle_servo);
//open up a serial port
Serial.begin(9600);
Serial.println("The test is about to begin");
//set leg to initial position:
AH = 0;
servo1.write(AH);
servo2.write(AK);
servo3.write(AA);
//Initialize the PIN connected to the encoder:
pinMode(phot_sens_pin,INPUT_PULLUP);
a_phase_init = digitalRead(phot_sens_pin); //Determine initial value of A_PHASE
a_phase_prev = a_phase_init;
//attachInterrupt(digitalPinToInterrupt(phot_sens_pin), time_interval, );
//Real quick, set new_time to micros so we dont have an infinite speed:
new_time = micros();
/*
for(int i=0; i<4; i++){
for(int j=0; j<2; j++){
Serial.print(String(hip_angles[i][j]) + '\t');
//Serial.print('\t');
}
Serial.print('\n');
}
*/
}
void loop() {
/*
//Check to see if the photosensor has clicked:
a_phase_new = digitalRead(phot_sens_pin); //measure current state of encoder (on/off)
//check to see if the encoder has entered a new state and compare to the intial state:
if((a_phase_new != a_phase_prev) && (a_phase_new != a_phase_init)){
prev_time = new_time;
motion_seg++; //we have entered the next segment of the motion
//normalize the step in the motion
step_det = motion_seg % (num_seg*2);
//check to see if we need to start a timer
if((step_det == 0) || (step_det == (num_seg/4)) || (step_det == (num_seg/2)) || (step_det == (num_seg/2)) || (step_det == num_seg)){
move_start_time = millis();
}
//Serial.print("poopsock");
}
else if((a_phase_new != a_phase_prev) && (a_phase_new == a_phase_init)){
new_time = micros();
time_int = new_time - prev_time;
walk_speed = wheel_circ_seg/time_int;
//Serial.print(walk_speed);
correct_count++;
if(walk_speed > 1){
//Serial.print('\t');
//Serial.print(time_int);
fuck_up_count++;
}
}
else{}
*/
walk_speed = (random(8,10))/10.;
//calculate time-length of each segment of the leg motion:
move_times[0] = (Lo2/(4*walk_speed))*1000;
move_times[1] = (Lo2/(4*walk_speed))*1000;
move_times[2] = (Lo2/(2*walk_speed))*1000;
move_times[3] = (Lo2/walk_speed)*1000;
/*
//Now we see what part of the movement we are in, there are 32 encoder clicks per full step motion:
//step_det = motion_seg % (num_seg*2);
*/
//Now check how much time has elapsed:
progress = millis() - move_start_time;
//tolerance (tol) for first segment:
if((AH >= (hip_angles[0][1]-3)) && (AH < hip_angles[0][1])){
AH = hip_angles[1][0];
Serial.print("T1");
reset_clock();
}
//first segment:
else if((AH >= hip_angles[0][0]) && (AH < hip_angles[0][1])){
if(progress <= move_times[0]){
AH = (progress/move_times[0])*(hip_angles[0][1]);
Serial.print("1");
}
}
//tol for second segment:
else if((AH >= (hip_angles[1][1]-2)) && (AH < hip_angles[1][1])){
AH = hip_angles[2][0];
reset_clock();
Serial.print("T2");
}
//second segment:
else if((AH >= hip_angles[1][0]) && (AH < hip_angles[1][1])){
if(progress <= move_times[1]){
AH = ((progress/move_times[1])*(hip_angles[1][1] - hip_angles[1][0])) + hip_angles[1][0];
Serial.print("2");
}
}
//tol for third segment:
else if((AH >= (hip_angles[2][1] - 2)) && (AH < hip_angles[2][1]) && (!fourth_seg)){
AH = hip_angles[2][1];
reset_clock();
fourth_seg = true;
Serial.print("T3");
}
//third segment:
else if((AH >= hip_angles[2][0]) && (AH < hip_angles[2][1]) && (!fourth_seg)){
if(progress <= move_times[2]){
AH = ((progress/move_times[2])*(hip_angles[2][1] - hip_angles[2][0])) + hip_angles[2][0];
Serial.print("3");
}
}
//tol for fourth segment:
else if((AH <= (hip_angles[3][1] + 2)) && (AH > hip_angles[3][1])){
AH = hip_angles[0][0];
reset_clock();
fourth_seg = false;
Serial.print("T4");
}
//fourth segment
else if((AH <= hip_angles[3][0]) && (AH > (hip_angles[3][1]))){
if(progress <= move_times[3]){
AH = (1-(progress/move_times[3]))*(hip_angles[3][0]);
Serial.print("4");
}
//Serial.print("4 kinda");
}
else{
Serial.print("bruh");
}
/*
//reset to first segment:
else if(AH >= (hip_angles[3][1] - 5)){
reset_clock();
AH = 0;
Serial.print("reset");
}
*/
Serial.print('\t');
Serial.print(AH);
Serial.print('\t');
Serial.print(walk_speed);
Serial.print('\t');
Serial.println(progress);
//Serial.println('\t');
//Serial.print(move_start_time);
//Serial.print('\t');
//Serial.println(move_times[1]);
//Serial.print(RH);
servo1.write(AH);
a_phase_prev = a_phase_new;
}
//resets the values of progress and move_start_time when a new motion segment begins
void reset_clock(){
progress = 0;
move_start_time = millis();
}