//--------------------Engine Process-----------------------------//
void engineProcess(float dt)
{
static int loopCounter;
tStopWatch sw;
loopCounter++;
LEDon();
DEBUG_LEDoff();
StopWatchInit(&sw);
MPU6050_ACC_get(AccData); // Getting Accelerometer data
unsigned long tAccGet = StopWatchLap(&sw);
MPU6050_Gyro_get(GyroData); // Getting Gyroscope data
unsigned long tGyroGet = StopWatchLap(&sw);
Get_Orientation(AccAngleSmooth, CameraOrient, AccData, GyroData, dt);
unsigned long tAccAngle = StopWatchLap(&sw);
// if we enable RC control
if (configData[9] == '1')
{
// Get the RX values and Averages
Get_RC_Step(Step, RCSmooth); // Get RC movement on all three AXIS
Step[PITCH] = Limit_Pitch(Step[PITCH], CameraOrient[PITCH]); // limit pitch to defined limits in header
}
// Pitch adjustments
//pitch_setpoint += Step[PITCH];
pitchRCOffset += Step[PITCH] / 1000.0;
pitch_angle_correction = constrain((CameraOrient[PITCH] + pitchRCOffset) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
pitch_setpoint += pitch_angle_correction; // Pitch return to zero after collision
// Roll Adjustments
//roll_setpoint += Step[ROLL];
rollRCOffset += Step[ROLL] / 1000.0;
// include the config roll offset which is scaled to 0 = -10.0 degrees, 100 = 0.0 degrees, and 200 = 10.0 degrees
roll_angle_correction = constrain((CameraOrient[ROLL] + rollRCOffset + Deg2Rad((configData[11] - 100) / 10.0)) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
roll_setpoint += roll_angle_correction; //Roll return to zero after collision
// if we enabled AutoPan on Yaw
if (configData[10] == '0')
{
//ADC1Ch13_yaw = ((ADC1Ch13_yaw * 99.0) + ((float)(readADC1(13) - 2000) / 4000.0)) / 100.0; // Average ADC value
//CameraOrient[YAW] = CameraOrient[YAW] + 0.01 * (ADC1Ch13_yaw - CameraOrient[YAW]);
yaw_setpoint = autoPan(Output[YAW], yaw_setpoint);
}
else
{
// Yaw Adjustments
yaw_setpoint += Step[YAW];
yawRCOffset += Step[YAW] / 1000.0;
}
#if 0
yaw_angle_correction = constrain((CameraOrient[YAW] + yawRCOffset) * R2D, -CORRECTION_STEP, CORRECTION_STEP);
yaw_setpoint += yaw_angle_correction; // Yaw return to zero after collision
#endif
unsigned long tCalc = StopWatchLap(&sw);
pitch_PID();
roll_PID();
yaw_PID();
unsigned long tPID = StopWatchLap(&sw);
unsigned long tAll = StopWatchTotal(&sw);
printcounter++;
//if (printcounter >= 500 || dt > 0.0021)
if (printcounter >= 500)
{
if (debugPrint)
{
print("Loop: %7d, I2CErrors: %d, angles: roll %7.2f, pitch %7.2f, yaw %7.2f\r\n",
loopCounter, I2Cerrorcount, Rad2Deg(CameraOrient[ROLL]),
Rad2Deg(CameraOrient[PITCH]), Rad2Deg(CameraOrient[YAW]));
}
if (debugSense)
{
print(" dt %f, AccData: %8.3f | %8.3f | %8.3f, GyroData %7.3f | %7.3f | %7.3f \r\n",
dt, AccData[X_AXIS], AccData[Y_AXIS], AccData[Z_AXIS], GyroData[X_AXIS], GyroData[Y_AXIS], GyroData[Z_AXIS]);
}
if (debugPerf)
{
print("idle: %5.2f%%, time[µs]: attitude est. %4d, IMU acc %4d, gyro %4d, angle %4d, calc %4d, PID %4d\r\n",
GetIdlePerf(), tAll, tAccGet, tGyroGet, tAccAngle, tCalc, tPID);
}
if (debugRC)
{
print(" RC2avg: %7.2f | RC3avg: %7.2f | RC4avg: %7.2f | RStep:%7.3f PStep: %7.3f YStep: %7.3f\r\n",
RCSmooth[ROLL], RCSmooth[PITCH], RCSmooth[YAW], Step[ROLL], Step[PITCH], Step[YAW]);
}
if (debugOrient)
{
print("Roll_setpoint:%12.4f | Pitch_setpoint:%12.4f | Yaw_setpoint:%12.4f\r\n",
roll_setpoint, pitch_setpoint, yaw_setpoint);
}
if (debugCnt)
{
print("Counter min %3d, %3d, %3d, max %4d, %4d, %4d, count %3d, %3d, %3d, usbOverrun %4d\r\n",
MinCnt[ROLL], MinCnt[PITCH], MinCnt[YAW],
MaxCnt[ROLL], MaxCnt[PITCH], MaxCnt[YAW],
IrqCnt[ROLL], IrqCnt[PITCH], IrqCnt[YAW],
usbOverrun());
}
if (debugAutoPan)
{
print("Pitch_output:%3.2f | Roll_output:%3.2f | Yaw_output:%3.2f | centerpoint:%4.4f\n\r",
Output[PITCH],
Output[ROLL],
Output[YAW],
centerPoint);
}
printcounter = 0;
}
LEDoff();
}
/* Continous_Mode_Test runs through the continuous mode test for the csd
* INPUTS:
* Contact_Addr, the address of the contact pio controller
* Contact_DDR, the address of the pio controllers data direction register
*/
void Continous_Mode_Test(void * Contact_Addr, void * Contact_DDR){
int Euler_data[3];
//Local variables
struct timeval Start_time, Time_stamp[NUMBER_OF_MEASURMENTS];
int IMU_roll_data[NUMBER_OF_MEASURMENTS], IMU_pitch_data[NUMBER_OF_MEASURMENTS], IMU_yaw_data[NUMBER_OF_MEASURMENTS];
//Set servo to wheel mode
Change_Mode(SERVO1, WHEEL, Data_out);
//Reset servo position
Get_Zero_Pos(Contact_Addr, Contact_DDR);
//Start turning the servo at a set rate
Set_Rate(SERVO1, TEST_SPEED, Data_out);
//start timer
gettimeofday(&Start_time, NULL);
//start taking measuremnts
int i = 0;
for(i = 0; i < NUMBER_OF_MEASURMENTS; i++){
//delay between
usleep(DELAY_BETWEEN_MEASURMENTS);
Get_Orientation(Euler_data);
//Sample IMU here
IMU_pitch_data[i] = Euler_data[0];
IMU_roll_data[i] = Euler_data[1];
IMU_yaw_data[i] = Euler_data[2];
//save timestamp
gettimeofday(&Time_stamp[i], NULL);
}
//Stop the servo
Set_Rate(SERVO1, STOP, Data_out);
double Run_time, Delta_time = 0;
double Cur_angle = 0;
double Prev_angle = 0;
double Delta_Angle = 0;
printf("ActualChange,MeasuredChangePitch,MeasuredChangeRoll,MeasuredChangeYaw,Absolute Angle,IMU Pitch,IMU Yaw,IMU Roll,Time\n");
for(i = 1; i < NUMBER_OF_MEASURMENTS; i++){
Delta_time = (double)((Time_stamp[i].tv_sec * 1000000 + Time_stamp[i].tv_usec) - (Time_stamp[i - 1].tv_sec * 1000000 + Time_stamp[i - 1].tv_usec))/(double)1000000;
Run_time = (double)((Time_stamp[i].tv_sec * 1000000 + Time_stamp[i].tv_usec) - (Start_time.tv_sec * 1000000 + Start_time.tv_usec))/(double)1000000;
//Delta_Angle_Expected = -atan(tan(2*PI*OMEGA*Run_time) - ((double)OMEGA/(double)HINGE_DISTANCE)*Delta_time*INCHS_PER_THREAD) - 2*PI*OMEGA*Run_time;
Cur_angle = atan((2*PI*OMEGA*Run_time*INCHS_PER_THREAD)/HINGE_DISTANCE);
Delta_Angle = Cur_angle - Prev_angle;
Prev_angle = Cur_angle;
printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",(Delta_Angle*180)/PI, (double)(IMU_pitch_data[i] - IMU_pitch_data[i-1] )/(double)16, (double)(IMU_roll_data[i] - IMU_roll_data[i-1] )/(double)16, (double)(IMU_yaw_data[i] - IMU_yaw_data[i-1])/(double)16, (Cur_angle*180)/PI, (double)IMU_pitch_data[i]/(double)16, (double)IMU_roll_data[i]/(double)16, (double)IMU_yaw_data[i]/(double)16, Run_time);
/*
printf("%d,", IMU_pitch_data[i]);
printf("%d,", IMU_roll_data[i]);
printf("%d,", IMU_yaw_data[i]);
printf("%d,", (int) Time_stamp[i].tv_sec);
printf("%d,", (int) Time_stamp[i].tv_usec);
*/
}
}
void Run_Static_Test(void * Contact_Addr, void * Contact_DDR)
{
int Cur_servo_pos = 1400;
int Servo_step = 0;
double Cur_angle = 0;
double Angle_step = 0;
int Current_Euler_data[3];
int Prev_Euler_data[3];
Change_Mode(SERVO1, WHEEL, Data_out);
Get_Zero_Pos(Contact_Addr, Contact_DDR);
//printf("Finished Zeroing servo\n");
usleep(1000*30);
Change_Mode(SERVO1, MULTI, Data_out);
usleep(1000*30);
uint8_t Error_array[15];
uint8_t Check_sum = (~(SERVO1 + MX_GOAL_LENGTH + MX_WRITE_DATA + 20)) & 0xFF;
uint8_t Data_packet_2[9] = {MX_START,MX_START,SERVO1,MX_GOAL_LENGTH,MX_WRITE_DATA, 20, 0, 0,Check_sum};
UART_WriteRead(Data_packet_2, sizeof(Data_packet_2), ONE_BYTE_READ, Error_array);
printf("Step Size,Roll,Pitch,Yaw\n");
double i;
int j;
for(i = 0.01; i < 0.5 ; i += 0.03)
{
Get_Orientation(Prev_Euler_data);
Angle_step = i;
for(j = 0; j<3;j++)
{
//printf("Entered second for loop\n");
Cur_angle += Angle_step;
//Find angles in radians
double Cur_angle_rad = Cur_angle * (PI/180);
double Angle_step_rad = Angle_step * (PI/180);
Servo_step = HINGE_DISTANCE * THREADS_PER_INCH * COUNTS_PER_360 * (tan(Cur_angle_rad) - tan(Cur_angle_rad - Angle_step_rad));
Cur_servo_pos += Servo_step;
//printf("Did math\n");
if(Cur_servo_pos > 24000){
printf("It broke :(\n At %lf\n",i);
}
Set_Position(SERVO1, Cur_servo_pos, Data_out);
usleep(3000*1000);
Get_Orientation(Current_Euler_data);
printf("%lf,%lf,%lf,%lf\n",Angle_step, (float)(Current_Euler_data[0] - Prev_Euler_data[0])/(float)16, (float)(Current_Euler_data[1] - Prev_Euler_data[1])/(float)16, (float)(Current_Euler_data[2] - Prev_Euler_data[2])/(float)16);
Prev_Euler_data[0] = Current_Euler_data[0];
Prev_Euler_data[1] = Current_Euler_data[1];
Prev_Euler_data[2] = Current_Euler_data[2];
}
Change_Mode(SERVO1, WHEEL, Data_out);
Get_Zero_Pos(Contact_Addr, Contact_DDR);
//printf("Finished Zeroing servo\n");
usleep(1000*300);
Change_Mode(SERVO1, MULTI, Data_out);
usleep(1000*300);
Cur_servo_pos = 1400;
Cur_angle = 0;
}
}
void Robot_Arm::Get_Robot_PosOri(Eigen::Matrix4d& T, Eigen::VectorXd & _P)
{
Eigen::Matrix3d R = T.topLeftCorner(3,3);
_P << T(0,3), T(1,3), T(2,3), Get_Orientation(R);
}
