int main(void) {
setup();
pwm_set_duty(0);
pwm_set_direction(0);
printf("%s\r\n", "STARTING LOOP");
float encoder_master_count = 0;
uint16_t current_ticks = 0;
uint16_t previous_ticks = spi_read_ticks();
float target_degs = 10;
float motor_current = 0; // current through motor in amperes
float pid_command = 0;
float theor_torque;
bitset(&IEC0, 2);
bitset(&IEC0, 6);
while (1) {
if (timer_flag(&timer2)) {
// Blink green light to show normal operation.
timer_lower(&timer2);
led_toggle(&led2);
}
if (timer_flag(&timer3)) {
timer_lower(&timer3);
led_toggle(&led3);
theor_torque = spring_model(degs); // Outputs theoretical torque predicted by spring model
cur_control.set_point = fabsf(theor_torque);
cur_control.neg_set_point = read_sign(theor_torque);
read_motor_current(&motor);
cur_control.position = convert_motor_torque(motor.current);
pid_command = PID_control(&cur_control);
pwm_duty = pid_to_pwm(pid_command, cur_control.neg_set_point);
}
if (!sw_read(&sw2)) {
// If switch 2 is pressed, the UART output terminal is cleared.
printf("%s", clear);
}
ServiceUSB();
current_ticks = spi_read_ticks();
encoder_master_count = encoder_counter(current_ticks, previous_ticks, encoder_master_count);
degs = count_to_deg(encoder_master_count);
previous_ticks = current_ticks;
pin_toggle(DEBUGD1); // Heartbeat signal
}
}
PUBLIC void run_nivel_control(void)
{
nivel_real = ALTURA_TANQUE - HCSR04_read(0);
// if (nivel_real < 0.0)
// {
// nivel_real = 0.00;
// }
pwm_out = PID_control(nivel_real, fl_set_point, &pid);
if(pwm_out < 0.0)
{
pwm_out = 0.0;
}
else if(pwm_out > 80.00)
{
pwm_out = 80;
}
pwmMCU_set_fduty(pwm_out);
}
void PIT0_IRQHandler(void)
{
DisableInterrupts;
TimeCount ++ ;
I1_Vol_present_value=ad_ave(ADC0, AD9,ADC_16bit,10);
I1_Vol_present_value=I1_Vol_present_value/65535*3.3;
U1_present_value=ad_ave(ADC0, AD10,ADC_16bit,10);
U1_present_value=U1_present_value/65535*3.3*16;
U2_present_value=ad_ave(ADC0, AD12,ADC_16bit,10);
U2_present_value=U2_present_value/65535*3.3*16;
sum+=I1_Vol_present_value;
PID_control();
if(TimeCount==3000)
{
TimeCount = 0 ;
sum/=3000;
I1_Vol_present_value=sum;
Refresh_Present();
sum=0;
if(U1_present_value>=24)
{
gpio_init(PORTC,3,GPO,0);
gpio_init(PORTC,4,GPO,0);
start_flag=0;
ftm_flag=0;
}
}
EnableInterrupts;
PIT_Flag_Clear(PIT0); //清中断标志位
}
int main(void)
{
PWM_Initialization();
TIM1->CCR2 = 1000;
TIM1->CCR3 = 1000;
Delay_1us(500000);
RCC_Configuration();
PushButton_Initialization();
LED_Initialization();
//LCD_Initialization();
//terminalBufferInitilization();
Delay_1us(100000);
SPI_Initialization();
Delay_1us(100000);
IMU_Initialization();
Delay_1us(100000);
Timer5_Initialization(); //Filter
Timer2_Initialization(); //Print
Timer4_Initialization(); //Read IMU
USART3_Configuration();
USART3_puts("\r\nHello World!\r\n");
while(1)
{
if(PushButton_Read()){
if(f.arm == 0){
f.arm = 1;
Delay_1us(500000);
}
else if(f.arm == 1){
f.arm = 0;
Delay_1us(500000);
}
}
if(f.imu == 1){
//LED3_Toggle();
readIMU(gyroRaw, GYRO_DATA_REGISTER);
gyro_remove_offset(gyroRaw);
readIMU(accRaw, ACC_DATA_REGISTER);
f.imu = 0;
// }
// if(f.filter == 1){
//LED4_Toggle();
Filter(gyroRaw, gyroAngle, accRaw, accAngle, Angle);
if(f.arm == 1){
PID_control(Angle);
}
else{
TIM1->CCR2 = 1000;
TIM1->CCR3 = 1000;
errorI = 0;
errorD = 0;
previous_error = 0;
}
//f.filter = 0;
}
strcmd_main();
//if(f.print == 1){
// sprintf(lcd_text_main,"%.4f %.4f %d", Angle[0], Angle[1], f.arm);
// //sprintf(lcd_text_main,"G: %.3f %.3f %.3f", EstV.G.X, EstV.G.Y, EstV.G.Z);
// LCD_DisplayStringLine(LINE(1), lcd_text_main);
// //sprintf(lcd_text_main,"A: %.3f %.3f %.3f", EstV.A.X, EstV.A.Y, EstV.A.Z);
// //sprintf(lcd_text_main,"A: %.3f %.3f", sqX_sqZ, EstV.GA.X*EstV.GA.X + EstV.GA.Z*EstV.GA.Z);
// // sprintf(lcd_text_main,"%.4f %.4f %.4f \n", gyroAngle[0], gyroAngle[1], gyroAngle[2]);
// sprintf(lcd_text_main,"%d ", gyroRaw[2]);
// LCD_DisplayStringLine(LINE(2), lcd_text_main);
// sprintf(lcd_text_main,"GA: %.3f %.3f %.3f", EstV.GA.X, EstV.GA.Y, EstV.GA.Z);
// LCD_DisplayStringLine(LINE(3), lcd_text_main);
//sprintf(lcd_text_main,"%.3f %.3f %.3f\n", EstV.G.Z, EstV.A.Z, EstV.GA.Z);
//LCD_DisplayStringLine(LINE(2), (uint8_t*)" Ming6842 @ github");
//terminalWrite(lcd_text_main);
//PRINT_USART();
//f.print = 0;
//}
}
while(1); // Don't want to exit
}
int translate(unsigned int *pru0data_int, unsigned int *leftPeriod, float *leftDuty, unsigned int *rightPeriod, float *rightDuty)
{
unsigned int prev_left_encoder_count, prev_right_encoder_count;
int i,j;
char *rec1 = "/mnt/bot1.txt", *rec2 = "/mnt/bot2.txt", *rec3="/mnt/bot3.txt";
int fd[2];
float trackRecord1[4], trackRecord2[4], trackRecord3[4], radius, deltaS, deltaX, deltaY, deltaTheta, Rvel, Lvel, Lerr, Rerr, Lp, LKp, Li, LKi, LKi_prev, Ld, LKd, LKd_prev, Rp, RKp, Ri, RKi, RKi_prev, Rd, RKd, RKd_prev;
readRecord(rec1, trackRecord1);
readRecord(rec2, trackRecord2);
readRecord(rec3, trackRecord3);
clock_t t, t2, t3;
radius = sqrt(pow((*(trackRecord1+1)- *(trackRecord2+1)),2)+pow((*(trackRecord1+2)- *(trackRecord2+2)),2));
printf("\n trackRecord1 %f \t %f \t %f \t %f\n trackRecord2 %f \t %f \t %f \t %f\n radius \t %f \n" ,*(trackRecord1+0), *(trackRecord1+1), *(trackRecord1+2), *(trackRecord1+3), *(trackRecord2+0), *(trackRecord2+1), *(trackRecord2+2), *(trackRecord2+3),radius);
//setHeading(pru0data_int, leftDuty, rightDuty);
prev_left_encoder_count = *(pru0data_int + 3);
prev_right_encoder_count = *(pru0data_int + 4);
*(pru0data_int + 1) = 1; //setting left direction to forward for pru0
*(pru0data_int + 2) = 1; //setting right direction to forward for pru0
motor_inB1_right(1);
motor_inA1_left(1);
motor_inA2_left(0);
motor_inB2_right(0);
while (radius > 850){
//t2 = clock();
//t3 = clock();
PID_control(pru0data_int, leftPeriod, leftDuty, rightPeriod, rightDuty);
printf(" left count: %u \t right count: %u \n \n", *(pru0data_int + 3), *(pru0data_int + 4));
//t = clock();
readRecord(rec2, trackRecord2);
deltaS = (float)((*(pru0data_int + 4)-prev_right_encoder_count) + ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_TRN_RES/2;
deltaTheta = (float)((*(pru0data_int + 4)-prev_right_encoder_count) - ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_ROT_RES/2;
*trackRecord2=*trackRecord2+1;
*(trackRecord2+1)=*(trackRecord2+1) + deltaS * cos(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+2)=*(trackRecord2+2) + deltaS * sin(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+3)=*(trackRecord2+3) + deltaTheta;
while (*(trackRecord2+3) > 2*M_PI)
*(trackRecord2+3) -= 2*M_PI;
/*if ( *(trackRecord2+1) < 2100 && *(trackRecord2+1) > 1800 && *(trackRecord2+2) < 2100 && *(trackRecord2+2) > 1800){
backstep(pru0data_int);
}
else {
writeRecord(rec2, trackRecord2);
//trackRecord1 = readRecord1(rec1);
}*/
writeRecord(rec2, trackRecord2);
readRecord(rec1, trackRecord1);
readRecord(rec2, trackRecord2);
readRecord(rec3, trackRecord3);
radius = sqrt(pow((trackRecord1[1]-trackRecord2[1]),2)+pow((trackRecord1[2]-trackRecord2[2]),2));
printf("\n trackRecord1 %f \t %f \t %f \t %f\n trackRecord2 %f \t %f \t %f \t %f\n radius \t %f \n" ,*(trackRecord1+0), *(trackRecord1+1), *(trackRecord1+2), *(trackRecord1+3), *(trackRecord2+0), *(trackRecord2+1), *(trackRecord2+2), *(trackRecord2+3),radius);
//t = clock() - t;
//printf ("\nIt took %d clicks (%f seconds).\n",t,((float)t)/CLOCKS_PER_SEC);
//setHeading(pru0data_int, leftDuty, rightDuty);
prev_left_encoder_count = *(pru0data_int + 3);
prev_right_encoder_count = *(pru0data_int + 4);
*(pru0data_int + 1) = 1; //setting left direction to forward for pru0
*(pru0data_int + 2) = 1; //setting right direction to forward for pru0
motor_inB1_right(1);
motor_inA1_left(1);
}
motor_inA2_left(1); //hard brake signal
motor_inB2_right(1);
motor_inA1_left(1);
motor_inB1_right(1);
sleep(1);
motor_inA1_left(0); // soft stop after hard brake
motor_inB1_right(0);
motor_inA2_left(0);
motor_inB2_right(0);
printf(" left count: %u \t right count: %u \n \n", *(pru0data_int + 3), *(pru0data_int + 4));
return 0;
}
int backstep(unsigned int *pru0data_int, unsigned int *leftPeriod, float *leftDuty, unsigned int *rightPeriod, float *rightDuty)
{
char *rec1 = "/mnt/bot1.txt", *rec2 = "/mnt/bot2.txt", *rec3 = "/mnt/bot3.txt";
float trackRecord1[4], trackRecord2[4], trackRecord3[4], dest_x, dest_y, dest_head, diff_in_theta, radius, deltaS, deltaX, deltaY, deltaTheta;
unsigned int prev_left_encoder_count, prev_right_encoder_count;
int fd[2], i, j, k;
prev_left_encoder_count = *(pru0data_int + 3);
prev_right_encoder_count = *(pru0data_int + 4);
readRecord(rec1, trackRecord1);
readRecord(rec2, trackRecord2);
readRecord(rec3, trackRecord3);
printf(" \n backstepping \n");
*(pru0data_int + 1) = 0; //setting left direction to backward for pru0
*(pru0data_int + 2) = 0; //setting right direction to backward for pru0
motor_inB1_right(0);
motor_inA1_left(0);
motor_inA2_left(1);
motor_inB2_right(1);
k = 0;
while ( k < 6) {
k++;
PID_control(pru0data_int, leftPeriod, leftDuty, rightPeriod, rightDuty);
}
motor_inB1_right(0);
motor_inA1_left(0);
motor_inA2_left(0);
motor_inB2_right(0);
deltaS = (float)(((*(pru0data_int + 4))-prev_right_encoder_count) + ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_TRN_RES/2;
deltaTheta = (float)(((*(pru0data_int + 4))-prev_right_encoder_count) - ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_ROT_RES/2;
*trackRecord2=*trackRecord2+1;
*(trackRecord2+1)=*(trackRecord2+1) + deltaS * cos(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+2)=*(trackRecord2+2) + deltaS * sin(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+3)=*(trackRecord2+3) + deltaTheta;
while (*(trackRecord2+3) > 2*M_PI)
*(trackRecord2+3) -= 2*M_PI;
writeRecord(rec2, trackRecord2);
prev_left_encoder_count = *(pru0data_int + 3);
prev_right_encoder_count = *(pru0data_int + 4);
readRecord(rec1, trackRecord1);
readRecord(rec2, trackRecord2);
diff_in_theta = - M_PI/4;
if (diff_in_theta < 0 && (int) (fabs(diff_in_theta)/ENC_ROT_RES)>0) { //rotate right
printf(" \n rotating right \n");
*(pru0data_int + 1) = 1; //setting left direction to forward for pru0
*(pru0data_int + 2) = 0; //setting right direction to backward for pru0
motor_inB1_right(0);
motor_inA1_left(1);
motor_inA2_left(0);
motor_inB2_right(1);
while (((int) (fabs(diff_in_theta)/ENC_ROT_RES) >= (abs(*(pru0data_int + 3) - prev_left_encoder_count))) && ((int) (fabs(diff_in_theta)/ENC_ROT_RES) >= (abs(*(pru0data_int + 4) - prev_right_encoder_count))))
{
PID_control(pru0data_int, leftPeriod, leftDuty, rightPeriod, rightDuty);
}
}
motor_inB1_right(0);
motor_inA1_left(0);
motor_inA2_left(0);
motor_inB2_right(0);
deltaS = (float)(((*(pru0data_int + 4))-prev_right_encoder_count) + ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_TRN_RES/2;
deltaTheta = (float)(((*(pru0data_int + 4))-prev_right_encoder_count) - ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_ROT_RES/2;
*trackRecord2=*trackRecord2+1;
*(trackRecord2+1)=*(trackRecord2+1) + deltaS * cos(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+2)=*(trackRecord2+2) + deltaS * sin(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+3)=*(trackRecord2+3) + deltaTheta;
while (*(trackRecord2+3) > 2*M_PI)
*(trackRecord2+3) -= 2*M_PI;
writeRecord(rec2, trackRecord2);
prev_left_encoder_count = *(pru0data_int + 3);
prev_right_encoder_count = *(pru0data_int + 4);
readRecord(rec1, trackRecord1);
readRecord(rec2, trackRecord2);
readRecord(rec3, trackRecord3);
printf(" \n moving forward \n");
*(pru0data_int + 1) = 1; //setting left direction to forward for pru0
*(pru0data_int + 2) = 1; //setting right direction to forward for pru0
motor_inB1_right(1);
motor_inA1_left(1);
motor_inA2_left(0);
motor_inB2_right(0);
k = 0;
while ( k < 6)
{
k++;
PID_control(pru0data_int, leftPeriod, leftDuty, rightPeriod, rightDuty);
}
motor_inB1_right(0);
motor_inA1_left(0);
motor_inA2_left(0);
motor_inB2_right(0);
deltaS = (float)(((*(pru0data_int + 4))-prev_right_encoder_count) + ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_TRN_RES/2;
deltaTheta = (float)(((*(pru0data_int + 4))-prev_right_encoder_count) - ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_ROT_RES/2;
*trackRecord2=*trackRecord2+1;
*(trackRecord2+1)=*(trackRecord2+1) + deltaS * cos(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+2)=*(trackRecord2+2) + deltaS * sin(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+3)=*(trackRecord2+3) + deltaTheta;
while (*(trackRecord2+3) > 2*M_PI)
*(trackRecord2+3) -= 2*M_PI;
writeRecord(rec2, trackRecord2);
return 0;
}
int setHeading(unsigned int *pru0data_int, unsigned int *leftPeriod, float *leftDuty, unsigned int *rightPeriod, float *rightDuty)
{
char *rec1 = "/mnt/bot1.txt", *rec2 = "/mnt/bot2.txt", *rec3 = "/mnt/bot3.txt";
float trackRecord1[4], trackRecord2[4], trackRecord3[4], dest_x, dest_y, dest_head, diff_in_theta, radius, deltaS, deltaX, deltaY, deltaTheta, Rvel, Lvel, Lerr, Rerr, Lp, LKp, Li, LKi, LKi_prev, Ld, LKd, LKd_prev, Rp, RKp, Ri, RKi, RKi_prev, Rd, RKd, RKd_prev;
int i, j;
unsigned int prev_left_encoder_count, prev_right_encoder_count;
prev_left_encoder_count = *(pru0data_int + 3);
prev_right_encoder_count = *(pru0data_int + 4);
readRecord(rec1, trackRecord1);
readRecord(rec2, trackRecord2);
readRecord(rec3, trackRecord3);
dest_x = (*(trackRecord1+1) + *(trackRecord2+1))/2.0;
dest_y = (*(trackRecord1+2) + *(trackRecord2+2))/2.0;
if (dest_y > *(trackRecord2+2)){
dest_head = atan2(dest_y - *(trackRecord2+2), dest_x - *(trackRecord2+1));
}
else if (dest_y < *(trackRecord2+2)){
dest_head = 2*M_PI + atan2(dest_y - *(trackRecord2+2), dest_x - *(trackRecord2+1));
printf("\ndest_head %f \t \n", dest_head*180.0/M_PI);
}
while (dest_head > 2*M_PI){
dest_head -= 2*M_PI;
}
diff_in_theta = dest_head - *(trackRecord2+3);
if (diff_in_theta > M_PI)
diff_in_theta = diff_in_theta - 2*M_PI;
//diff_in_theta = M_PI/4;
printf("\n dest_x %f \t dest_y %f \t present_x %f \t present_y %f \t dest_head %f \t present_head %f \t \n", dest_x, dest_y, *(trackRecord2+1), *(trackRecord2+2), dest_head*180.0/M_PI, *(trackRecord2+3)*180.0/M_PI);
printf("\n %d \t %d \n", (int) (fabs(diff_in_theta)/ENC_ROT_RES), (int) (fabs(diff_in_theta)/ENC_ROT_RES));
if (diff_in_theta > 0 && (int) (fabs(diff_in_theta)/ENC_ROT_RES) > 0) { //rotate left
printf(" \n rotating left \n");
*(pru0data_int + 1) = 0; //setting left direction to backward for pru0
*(pru0data_int + 2) = 1; //setting right direction to forward for pru0
motor_inB1_right(1);
motor_inA1_left(0);
motor_inA2_left(1);
motor_inB2_right(0);
while (((int) (fabs(diff_in_theta)/ENC_ROT_RES) >= (abs(*(pru0data_int + 3) - prev_left_encoder_count))) && ((int) (fabs(diff_in_theta)/ENC_ROT_RES) >= (abs(*(pru0data_int + 4) - prev_right_encoder_count))))
{
PID_control(pru0data_int, leftPeriod, leftDuty, rightPeriod, rightDuty);
//printf(" left count: %u \t right count: %u \n \n", (*(pru0data_int + 3)), (*(pru0data_int + 4)));
/*deltaS = ((*(pru0data_int + 4))-prev_left_encoder_count + (*(pru0data_int + 3))-prev_right_encoder_count)*ENC_TRN_RES/2;
deltaTheta = ((*(pru0data_int + 4))-prev_left_encoder_count - (*(pru0data_int + 3))-prev_right_encoder_count)*ENC_ROT_RES/2;
*trackRecord2=*trackRecord2+1;
*(trackRecord2+1)=*(trackRecord2+1) + deltaS * cos(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+2)=*(trackRecord2+2) + deltaS * sin(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+3)=*(trackRecord2+3) + deltaTheta;
writeRecord(rec2, trackRecord2);*/
}
}
if (diff_in_theta < 0 && (int) (fabs(diff_in_theta)/ENC_ROT_RES)>0) { //rotate right
printf(" \n rotating right \n");
*(pru0data_int + 1) = 1; //setting left direction to forward for pru0
*(pru0data_int + 2) = 0; //setting right direction to backward for pru0
motor_inB1_right(0);
motor_inA1_left(1);
motor_inA2_left(0);
motor_inB2_right(1);
while (((int) (fabs(diff_in_theta)/ENC_ROT_RES) >= (abs(*(pru0data_int + 3) - prev_left_encoder_count))) && ((int) (fabs(diff_in_theta)/ENC_ROT_RES) >= (abs(*(pru0data_int + 4) - prev_right_encoder_count))))
{
PID_control(pru0data_int, leftPeriod, leftDuty, rightPeriod, rightDuty);
}
}
motor_inB1_right(0);
motor_inA1_left(0);
motor_inA2_left(0);
motor_inB2_right(0);
deltaS = (float)(((*(pru0data_int + 4))-prev_right_encoder_count) + ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_TRN_RES/2;
deltaTheta = (float)(((*(pru0data_int + 4))-prev_right_encoder_count) - ((*(pru0data_int + 3))-prev_left_encoder_count))*ENC_ROT_RES/2;
*trackRecord2=*trackRecord2+1;
*(trackRecord2+1)=*(trackRecord2+1) + deltaS * cos(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+2)=*(trackRecord2+2) + deltaS * sin(*(trackRecord2+3)+deltaTheta);
*(trackRecord2+3)=*(trackRecord2+3) + deltaTheta;
while (*(trackRecord2+3) > 2*M_PI)
*(trackRecord2+3) -= 2*M_PI;
writeRecord(rec2, trackRecord2);
return 0;
}
