main.c
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <machine.h>
#include "platform.h"
#include "r_switches.h"
#include "./encoders/encoder_1.h" /* encoder header file */
#include "./encoders/encoder_2.h"
#include "./imu/SetupAHRS.h"
#include "./imu/AHRS.h"
#include "./imu/imu.h"
#include "CMT.h" /* Control time unit */
#include "PWM.h" /* Output control */
#include "./imu/i2c.h"
#include "./sensCurr/sens_current.h"
#include "PID.h" /* PID struct and method */
#include "utility.h"
#include "filters.h"
#include "./wifi/bike_uart.h"
#include "./wifi/bike_command.h"
#include "S12ADC.h"
unsigned char Enable_lcd_enc = 1;
/* This variable are used to store data used trough many function */
float result_adc;
float ref_curr=0;
short PID_ON=0;
posReg position;
extern struct encoder_data enc;
extern struct encoder_2_data enc_2;
/*AHRS per IMU*/
AHRS_out ahrs;
extern unsigned int TX_Flag_CSTART;
extern unsigned int TX_Flag_BSTART;
extern unsigned int TX_Flag_FORCED;
/***************************************
* Function name: main
* Description : Main program function
* Arguments : none
* Return value : none
****************************************/
void main(void) {
/* Local variable */
float voltaggio;
float appoggio=0, appoggio1=0, appoggio2=0;
float speed, current, spd_mot, spd_ruota, speed_ref, balance;
float c0_filt;
float cntrlVoltage[2]= {0,0}; //offset[2];
short res=0;
struct sens_cur SdC0;
speed_ref=0.00;
//speed_ref = 0.56; // m/s = 2 km/h (conv 1:3.6) //speed_ref = 1.11; // m/s = 4 km/h
//speed_ref=1.67; // m/s = 6 km/h //speed_ref= 2.22; // ms = 8 km/h float tens=0;
float offset;
float newVsen=0; //current value read unsigned int old=0;
unsigned int new=0;
unsigned int i=0;
unsigned short int state=0;
uint8_t pc_buffer[100];
/* PIDs struct, decleared one at time and stored in array */
pidSt pos0, pos1, spd0, spd1, cur0, cur1, bal0, bal1;
pidSt * pid_pos[2] = {&pos0, &pos1};
pidSt * pid_speed[2] = {&spd0, &spd1};
pidSt * pid_current[2] = {&cur0, &cur1};
pidSt * pid_balance[2] = {&bal0, &bal1};
uint8_t lcd_buffer[20];
/* Initialize LCD */
lcd_initialize();
/* Clear LCD */
lcd_clear();
/* Display message on LCD */
lcd_display(LCD_LINE1, " RENESAS ");
lcd_display(LCD_LINE2, " YRDKRX63N ");
/* The three pushbuttons on the YRDK board are tied to interrupt lines, set them up here */
R_SWITCHES_Init();
/* Initialize and start the Compare Match Timer */
CMT_initialize();
/* initializing PWM 1 2 => MTU4*/
PWM_Init(1); //trazione PWM_Init(2); //sterzo
/* Initialize of control struct and direction pin */
//init_controlstruct();
init_dirPin();
/*initialize Encoder 1 2 and their needed peripherials */
encoder_1_init();
encoder_2_init();
/* Initialization of PID struct, 3 for each motor. */
//init_pid_pos(pid_pos[0]);
init_pid_pos(pid_pos[1]);
init_pid_speed(pid_speed[0]);
init_pid_speed_1(pid_speed[1]);
//init_pid_curr(pid_current[0]);
init_pid_curr(pid_current[1]);
init_pid_balance(pid_balance[0]);
/*initialization of Wi-Fi*/
uart_init();
/*init i2c */
i2c_init();
/* init IMU */
Setup_MARG(&ahrs);
/*Initialize AD 12bit Converter */
S12ADC_init();
/* init sens current */
sens_curr_init(&SdC0, 0);
unsigned int t;
for (int i=0; i<400; i++) { t=general_timer_mS;
while (general_timer_mS < t+10);
if(Manage_ADC() == 1){
/* Fetch the results from the ADC12 */
newVsen = S12ADC_read( );
/* Using a software filter*/
offset = MovingAvgFilter(newVsen,2);
} Tc_5mS = false;
// Fetch the encoder state for motor 1 if(Manage_ADC() == 1)
{
/*Fetch the results from the ADC12 */
newVsen = S12ADC_read( );
/* Using a software filter */
//tens = MovingAvgFilter(Low_pass_filter(newVsen),2);
//tens = Low_pass_filter(newVsen);
//tens=newVsen;
tens = MovingAvgFilter(newVsen,2);
/*from voltage to ampere
sensitivity : max_current = k : voltage*/
result_adc = (tens-offset)*8.19;
} }
if (Tc_10mS) { Tc_10mS = false;
Query_Enc_2();
appoggio1=enc_2.position_in_pigreek_radians_units * 3.1415 ; if (res == 1) {
lcd_display(LCD_LINE7, "error SdC");
}
//calculate pid result channel 2 if(PID_ON==1){
//controllo con pid di coppia, velocit`a e posizione //speed = calcPID ( 0.174533, appoggio1 , pid_pos[1]);
//current = calcPID ( 3.0, enc_2.speed_in_rad_per_sec , // pid_speed[1]);
//cntrlVoltage[1] = calcPID ( 0.8, result_adc, pid_current[1]);
//cntrlVoltage[1] = 12;
//controllo con PID di posizione, poich`e non `e possibile controllare lo sterzo attraverso il PID di coppia.
cntrlVoltage[1] = calcPID ( balance , appoggio1 , pid_pos[1]);
appoggio2 = cntrlVoltage[1];
}
else{cntrlVoltage[1]=0;}
cntrlVoltage[1] = rescale(cntrlVoltage[1], 2);
if(TX_Flag_FORCED==1){
if (Tc_20mS) { Tc_20mS = false;
Query_Enc_1();
if(state == 1) {
res = read_curr(&SdC0);
c0_filt = MovingAvgFilter(SdC0.curr/1000, 0);
state = 0;
} else {
sens_curr_init(&SdC0, 0);
state = 1;
}
appoggio = (enc.position_in_pigreek_radians_units * 3.1415);
//calculate pid result channel 1
//speed = calcPID ( position.gradual[0], appoggio, pid_pos[0]);
spd_mot = enc.speed_in_rad_per_sec;
// ruota dentata minore : maggiore = 36 mm : 54 mm = //=54/36 = 1,5
spd_ruota = spd_mot * 1.5;
//diametro ruota 355.6 mm
voltaggio = - calcPID ( speed_ref, spd_ruota*0.3556/2 , pid_speed[0]);
if (voltaggio > 0) { voltaggio = 0; } cntrlVoltage[0] = voltaggio;
//rescale signal to pwm and update
cntrlVoltage[0] = rescale(cntrlVoltage[0], 1);
if(TX_Flag_FORCED==1){
Tc_50mS = false;
new=get_ms();
i=new-old;
old=new;
Read_MARG(&ahrs);
balance = calcPID( 0.0, ahrs.ahrs_data.RollRad, pid_balance[0]);
}
if (Tc_100mS) {
Tc_100mS = false;
sprintf((char*)lcd_buffer, "time = %d", i);
lcd_display(LCD_LINE8, lcd_buffer);
// sprintf((char*)lcd_buffer, "%f", speed);
// lcd_display(LCD_LINE4, lcd_buffer);
// sprintf((char*)lcd_buffer, "%f", enc_2.speed_in_rad_per_sec);
// lcd_display(LCD_LINE5, lcd_buffer);
// sprintf((char*)lcd_buffer, "%.3f", result_adc);
// lcd_display(LCD_LINE6, lcd_buffer);
// sprintf((char*)lcd_buffer, "%f",cntrlVoltage[1] );
// lcd_display(LCD_LINE7, lcd_buffer);
// sprintf((char *)lcd_buffer, " p= %.1f" , // enc_2.position_in_degree);
// lcd_display(LCD_LINE6, lcd_buffer);
//Read_Enc_1();
//Read_Enc_2();
IMU_Display(ahrs.ahrs_data);
if(TX_Flag_CSTART == 1) { //sprintf((char *)pc_buffer,
//sprintf((char *)pc_buffer, "%u\t%.3f\t%.3f\t%.3f\n" , get_ms(),ahrs.ahrs_data.RollDeg,
// ahrs.ahrs_data.PitchDeg, ahrs.ahrs_data.YawDeg);
//sprintf((char *)pc_buffer, "%u\t%.3f\t%.3f\t%.3f\n" ,
get_ms(), result_adc, appoggio2, enc_2.speed_in_rad_per_sec );
//sprintf((char *)pc_buffer, "%u\t%.3f\t\n" , get_ms(),
enc_2.position_in_degree );\\
sprintf((char *)pc_buffer, "%u\t%.3f\t%.3f\n" , get_ms(), enc_2.position_in_degree, ahrs.ahrs_data.RollDeg );
send_data((char*)pc_buffer, str_length((char*)pc_buffer));
}
if(TX_Flag_BSTART==1){
speed_ref = 2.22; // m/s = 8 km/h (conv 1:3.6) } else {speed_ref=0.00;}
}
} /* end while */
} /* End of function main() */
/*************************************************
* Function name: sw1_callback
* Description : Callback function that is executed when SW1 is pressed.
* Called by sw1_isr in r_switches.c
* Arguments : none
* Return value : none
**************************************************/
void sw1_callback(void) {
PID_ON=1;
} /* End of function sw1_callback() */
/*************************************************
* Function name: sw2_callback
* Description : Callback function that is executed when SW2 is pressed.
* Called by sw2_isr in r_switches.c
* Arguments : none
* Return value : none
**************************************************/
void sw2_callback(void) {
PID_ON=0;
} /* End of function sw2_callback() */
/**************************************************
* Function name: sw3_callback
* Description : Callback function that
is executed when SW3 is pressed.
* Called by sw3_isr in r_switches.c
* Arguments : none
* Return value : none
**************************************************/
void sw3_callback(void) {
nop(); /* Add your code here. Note: this is executing inside an ISR. */
} /* End of function sw3_callback() */