// approach control,
void approach_control(double time, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
double phi = navData_ptr->phi; // roll angle
double theta = navData_ptr->the - base_pitch_cmd; // subtract theta trim value to convert to delta coordinates
double p = sensorData_ptr->imuData_ptr->p; // roll rate
double q = sensorData_ptr->imuData_ptr->q; // pitch rate
double ias = sensorData_ptr->adData_ptr->ias_filt; // filtered airspeed
double ias_cmd, phi_cmd, theta_cmd;
controlData_ptr->theta_cmd = approach_theta - base_pitch_cmd; // delta theta command [rad]
controlData_ptr->ias_cmd = approach_speed; // absolute airspeed command [m/s]
controlData_ptr->phi_cmd = pilot_phi(sensorData_ptr); // phi from the pilot stick
theta_cmd = controlData_ptr->theta_cmd;
ias_cmd = controlData_ptr->ias_cmd;
phi_cmd = controlData_ptr->phi_cmd;
controlData_ptr->dthr = speed_control(ias_cmd, ias, TIMESTEP); // Throttle [ND 0-1]
controlData_ptr->l1 = 0; // L1 [rad]
controlData_ptr->l2 = roll_control(phi_cmd, phi, p, TIMESTEP, 0); // L2 [rad]
controlData_ptr->l3 = pitch_control(theta_cmd, theta, q, TIMESTEP, 0); // L3 [rad]
controlData_ptr->l4 = controlData_ptr->l3 + controlData_ptr->l2; // L4 [rad]
controlData_ptr->r1 = 0; // R1 [rad]
controlData_ptr->r2 = -1*controlData_ptr->l2; // R2 [rad]
controlData_ptr->r3 = controlData_ptr->l3; // R3 [rad]
controlData_ptr->r4 = controlData_ptr->r3 + controlData_ptr->r2; // R4 [rad]
}
void pilot_flying_inner(double time, double ias_cmd, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
double phi = navData_ptr->phi; // roll angle
double theta = navData_ptr->the - base_pitch_cmd; // subtract theta trim value to convert to delta coordinates
double p = sensorData_ptr->imuData_ptr->p; // roll rate
double q = sensorData_ptr->imuData_ptr->q; // pitch rate
double ias = sensorData_ptr->adData_ptr->ias_filt; // filtered airspeed
double phi_cmd, theta_cmd;
controlData_ptr->phi_cmd = pilot_phi(sensorData_ptr) + controlData_ptr->phi_cmd; // phi from the pilot stick + guidance
controlData_ptr->theta_cmd = pilot_theta(sensorData_ptr) + controlData_ptr->theta_cmd; // theta from the pilot stick + guidance
controlData_ptr->ias_cmd = ias_cmd;
theta_cmd = controlData_ptr->theta_cmd;
phi_cmd = controlData_ptr->phi_cmd;
controlData_ptr->dthr = speed_control(ias_cmd, ias, TIMESTEP); // Throttle [ND 0-1]
controlData_ptr->l1 = 0; // L1 [rad]
controlData_ptr->l2 = roll_control(phi_cmd, phi, p, TIMESTEP, 1); // L2 [rad]
controlData_ptr->l3 = pitch_control(theta_cmd, theta, q, TIMESTEP, 1); // L3 [rad]
controlData_ptr->l4 = 0; // L4 [rad]
controlData_ptr->r1 = 0; // R1 [rad]
controlData_ptr->r2 = -1*controlData_ptr->l2; // R2 [rad]
controlData_ptr->r3 = controlData_ptr->l3; // R3 [rad]
controlData_ptr->r4 = 0; // R4 [rad]
}
void flare_control(double time, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
double ramp_time = 6.0;
double min_speed = 8;
double cut_time = 5;
double phi = navData_ptr->phi; // roll angle
double theta = navData_ptr->the - base_pitch_cmd; // subtract theta trim value to convert to delta coordinates
double p = sensorData_ptr->imuData_ptr->p; // roll rate
double q = sensorData_ptr->imuData_ptr->q; // pitch rate
double ias = sensorData_ptr->adData_ptr->ias_filt; // filtered airspeed
static int t1_latched = FALSE; // time latching
static double t1 = 0;
double ias_cmd, phi_cmd, theta_cmd;
if(time <= ramp_time){ // ramp commands
ias_cmd = approach_speed - time*((approach_speed - flare_speed)/ramp_time);
theta_cmd = approach_theta + time*((flare_theta - approach_theta)/ramp_time);
}
else{ // final values
ias_cmd = flare_speed;
theta_cmd = flare_theta;
}
if(ias < min_speed){ // engine cutoff
if(t1_latched == FALSE){
t1 = time;
t1_latched = TRUE;
}
if((time-t1) > cut_time){
ias_cmd = -100;
}
}
else{
t1_latched = FALSE;
}
controlData_ptr->theta_cmd = theta_cmd - base_pitch_cmd; // delta theta command [rad]
controlData_ptr->ias_cmd = ias_cmd; // absolute airspeed command [m/s]
controlData_ptr->phi_cmd = 0; // wings level
theta_cmd = controlData_ptr->theta_cmd;
ias_cmd = controlData_ptr->ias_cmd;
phi_cmd = controlData_ptr->phi_cmd;
controlData_ptr->dthr = speed_control(ias_cmd, ias, TIMESTEP); // Throttle [ND 0-1]
controlData_ptr->l1 = 0; // L1 [rad]
controlData_ptr->l2 = roll_control(phi_cmd, phi, p, TIMESTEP, 0); // L2 [rad]
controlData_ptr->l3 = pitch_control(theta_cmd, theta, q, TIMESTEP, 0); // L3 [rad]
controlData_ptr->l4 = controlData_ptr->l3 + controlData_ptr->l2; // L4 [rad]
controlData_ptr->r1 = 0; // R1 [rad]
controlData_ptr->r2 = -1*controlData_ptr->l2; // R2 [rad]
controlData_ptr->r3 = controlData_ptr->l3; // R3 [rad]
controlData_ptr->r4 = controlData_ptr->r3 + controlData_ptr->r2; // R4 [rad]
}
void open_loop(double time, double ias_cmd, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
double ias = sensorData_ptr->adData_ptr->ias_filt; // filtered airspeed
double roll_incp = sensorData_ptr->inceptorData_ptr->roll;
double pitch_incp = sensorData_ptr->inceptorData_ptr->pitch;
controlData_ptr->ias_cmd = ias_cmd;
controlData_ptr->dthr = speed_control(ias_cmd, ias, TIMESTEP); // Throttle [ND 0-1]
controlData_ptr->l1 = 0; // L1 [rad]
controlData_ptr->l2 = roll_incp*L2_MAX; // L2 [rad]
controlData_ptr->l3 = -1*pitch_incp*(L3_MAX-15*D2R); // L3 [rad]
controlData_ptr->l4 = 0; // L4 [rad]
controlData_ptr->r1 = 0; // R1 [rad]
controlData_ptr->r2 = -1*controlData_ptr->l2; // R2 [rad]
controlData_ptr->r3 = controlData_ptr->l3; // R3 [rad]
controlData_ptr->r4 = 0; // R4 [rad]
}
void odom_callback(const nav_msgs::Odometry odom_ref){
std::cout<<"segundos: "<<odom_ref.header.stamp.sec<<" nanosegundos: "<<odom_ref.header.stamp.nsec<<"--> en segundos: "<<(float)odom_ref.header.stamp.nsec/1000000000<<std::endl;
double tiempo = (double)odom_ref.header.stamp.sec+(double)odom_ref.header.stamp.nsec/1000000000;
double error_temp = tiempo-t_prev_loop;
std::cout<<"Tiempo entre actualizaciones: "<<error_temp<<std::endl;
odom_rv = odom_ref.twist.twist;
std::cout<<"------------------------------"<<std::endl;
std::cout<<"ODOM UPDATE"<<std::endl;
//std::cout<<odom_ref<<std::endl;
std::cout<<"ODOM POSITION"<<std::endl;
std::cout<<odom_ref.pose.pose.position<<std::endl;
std::cout<<"ODOM VEL"<<std::endl;
std::cout<<"("<<odom_rv.linear<<", "<<odom_rv.angular.z<<" )"<<std::endl;
std::cout<<"elemento vector"<<ref_pos+1<<std::endl;
//TODO
//std_position(odom_ref);
// std::cout<<"FLYING STATE"<<std::endl;
// std::cout<<bebop_fst<<std::endl;
float e_z,e_x,e_y, z_ref,x_ref, y_ref;
x_ref=vec_ref.at(ref_pos).x;
y_ref=vec_ref.at(ref_pos).y;
z_ref=vec_ref.at(ref_pos).z;
std::cout<<"REFERENCIA: ("<<x_ref<<", "<<y_ref<<", "<<z_ref<<" )"<<std::endl;
e_x=x_ref-odom_ref.pose.pose.position.x;
e_y=y_ref-odom_ref.pose.pose.position.y;
//geometry_msgs::Twist twistter;
e_z=z_ref-odom_ref.pose.pose.position.z;
if (flag_fst==true){
std::cout<<"SPEED_CONTROL ACTIVATED"<<std::endl;
if(ref_pos<vec_ref.size()-1){
if(/*(e_y<=0.1)&&*/(e_x<=0.1)){
ref_pos++;
}
}
speed_control(e_x,e_y,e_z,(float)error_temp);
}
t_prev_loop=tiempo;
}
int main()
{
int sockfd;
float brightness;
struct sockaddr_in server_addr;
char response[MAX_SIZE];
char cmd[MAX_SIZE];
char cmd2[MAX_SIZE];
FILE *fp, *fp2;
char component_string[MAX_SIZE];
char value_string[MAX_SIZE];
int value_int;
mraa_pwm_context speed_pwm_in1, speed_pwm_in2, turn_pwm;
speed_pwm_in1 = mraa_pwm_init(3);
speed_pwm_in2 = mraa_pwm_init(5);
turn_pwm = mraa_pwm_init(6);
if (speed_pwm_in1 == NULL || speed_pwm_in2 == NULL || turn_pwm == NULL) {
fprintf(stderr, "Failed to initialized.\n");
return 1;
}
mraa_pwm_period_us(speed_pwm_in1,870); //1150Hz
mraa_pwm_enable(speed_pwm_in1, 1);
mraa_pwm_period_us(speed_pwm_in2,870);
mraa_pwm_enable(speed_pwm_in2, 1);
mraa_pwm_period_ms(turn_pwm,20);
mraa_pwm_enable(turn_pwm, 1);
mraa_pwm_write(speed_pwm_in1, 1.0f);
mraa_pwm_write(speed_pwm_in2, 1.0f);
mraa_pwm_write(turn_pwm, CENTER);
//Set the protocol to mqtt and
//restart iotkit-agent to ensure that it is running.
system("iotkit-admin protocol mqtt");
system("systemctl restart iotkit-agent");
//create a socket with the following attributes:
socklen_t len = sizeof(server_addr);
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
bzero(&server_addr,sizeof(server_addr));
//Name of the socket as agreed with the server
server_addr.sin_family = AF_INET;
server_addr.sin_addr.s_addr = inet_addr(localhost);
server_addr.sin_port = htons(41235);
if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
fprintf(stderr, "Failed to bind");
return 1;
}
while(1)
{
//Use recvfrom function to send JSON message to iotkit-agent
if(recvfrom(sockfd, response, MAX_SIZE, 0, (struct sockaddr *)&server_addr, &len) < 0)
{
fprintf(stderr, "Failed to receive actuation\n");
return 1;
}
//printf("Received JSON message: %s\n", response);
//---This part extracts the component name----------------
snprintf(cmd, sizeof(cmd), "echo '%s' | sed 's/\"/ /g' | tr ' ' '\n' | awk '/component/{getline; getline; print}'", response);
fp = popen(cmd, "r");
if (fp == NULL) {
fprintf(stderr, "Failed to run command\n" );
exit(1);
}
while (fgets(component_string, sizeof(component_string)-1, fp) != NULL) {
}
pclose(fp);
//---This part extracts the value-------------------------
snprintf(cmd2, sizeof(cmd2), "echo '%s' | sed 's/\"/ /g' | tr ' ' '\n' | awk '/value/{getline; getline; print}'", response);
fp2 = popen(cmd2, "r");
if (fp2 == NULL) {
fprintf(stderr, "Failed to run command\n" );
exit(1);
}
while (fgets(value_string, sizeof(value_string)-1, fp) != NULL) {
}
pclose(fp2);
//printf(value_string);
//---This part is for actuation
if (!strcmp(component_string, "vehicleControl\n")) {
if (!strcmp(value_string, "forward\n")) {
printf("Going forward for 1 second.\n\n");
mraa_pwm_write(turn_pwm, CENTER); //straight
usleep(100000);
speed_control(speed_pwm_in1, speed_pwm_in2, 100);
sleep(1);
speed_control(speed_pwm_in1, speed_pwm_in2, 0);
}
else if (!strcmp(value_string, "reverse\n")) {
printf("Going backward for 1 second.\n\n");
mraa_pwm_write(turn_pwm, CENTER); //reverse
usleep(100000);
speed_control(speed_pwm_in1, speed_pwm_in2, -100);
sleep(1);
speed_control(speed_pwm_in1, speed_pwm_in2, 0);
}
else if (!strcmp(value_string, "left\n")) {
printf("Making a left turn.\n\n");
mraa_pwm_write(turn_pwm, CENTER - 0.015f); //left
usleep(100000);
speed_control(speed_pwm_in1, speed_pwm_in2, 100);
sleep(1);
speed_control(speed_pwm_in1, speed_pwm_in2, 0);
}
else if (!strcmp(value_string, "right\n")) {
printf("Making a right turn.\n\n");
mraa_pwm_write(turn_pwm, CENTER + 0.015f); //right
usleep(100000);
speed_control(speed_pwm_in1, speed_pwm_in2, 100);
sleep(1);
speed_control(speed_pwm_in1, speed_pwm_in2, 0);
}
else
fprintf(stderr, "Wrong actuation command received!\n");
}
}
close(sockfd);
return 0;
}
int main(){
/**********************************************************************/
/**********************************************************************/
/* Begin. */
mraa_uart_context uart;
uart = mraa_uart_init(0);
mraa_uart_set_baudrate(uart, 115200);
char buffer[] = "hhh";
char flush[]="xxxxxxxxxxxx";
char str[] = "HELLO";
if (uart == NULL) {
fprintf(stderr, "UART failed to setup\n");
printf("UART failed");
return 1;
}
printf("firstavail:%d\n",mraa_uart_data_available(uart,0));
while (mraa_uart_data_available(uart,0))
{
mraa_uart_read(uart, flush, sizeof(uart));
printf("Flush: %c %c %c %c %c %c %c %c",flush[0], flush[1], flush[2],flush[3],flush[4],flush[5],flush[6],flush[7]);
usleep(150);
}
printf("available: %d",mraa_uart_data_available(uart,0));
char speed_user_input[MAXBUFSIZ];
char turn_user_input[MAXBUFSIZ];
mraa_pwm_context speed_pwm_in1, speed_pwm_in2, turn_pwm;
speed_pwm_in1 = mraa_pwm_init(3);
speed_pwm_in2 = mraa_pwm_init(5);
turn_pwm = mraa_pwm_init(6);
if (speed_pwm_in1 == NULL || speed_pwm_in2 == NULL || turn_pwm == NULL) {
fprintf(stderr, "Failed to initialized.\n");
return 1;
}
mraa_pwm_period_us(speed_pwm_in1,870); //1150Hz
mraa_pwm_enable(speed_pwm_in1, 1);
mraa_pwm_period_us(speed_pwm_in2,870);
mraa_pwm_enable(speed_pwm_in2, 1);
mraa_pwm_period_ms(turn_pwm,20);
mraa_pwm_enable(turn_pwm, 1);
mraa_pwm_write(turn_pwm, CENTER);
mraa_pwm_write(speed_pwm_in1, 1.0f);
mraa_pwm_write(speed_pwm_in2, 1.0f);
int n = 5;
int spd = 0;
char direction = 'C';
char cross_clr = 'B';
float angle = 0.0f;
while (1)
{
//sleep(2);
//readCharAry(uart,flush);
spd = 50;
mraa_uart_read(uart, buffer, 1);
if(buffer[0]=='\n')
{
printf("new line ");
mraa_uart_read(uart, buffer, 1);
if(buffer[0]=='\n')
{
printf("new line ");
mraa_uart_read(uart, buffer, 1);
}
}
mraa_uart_read(uart, buffer+1, 1);
if(buffer[1]=='\n')
{
buffer[0] = 'h';
buffer[1] = 'h';
}
int sign = 0;
if(cross_clr == 'M')
{
speed_control(speed_pwm_in1, speed_pwm_in2, 0.0f);
sleep(1);
char* nearestBeaconID = (char*)malloc(5);
getStrongestBeacon(nearestBeaconID);
printf("the nearestBeaconID is: %s\n", nearestBeaconID);
sign = 1;
angle = CENTER + 0.015f;
mraa_pwm_write(turn_pwm, angle);
usleep(15000);
speed_control(speed_pwm_in1, speed_pwm_in2, 54);
printf("speed: %d",spd);
usleep(5500000);
speed_control(speed_pwm_in1, speed_pwm_in2, 0.0f);
//sleep(1);
mraa_uart_write(uart, str, 1);
//while(!mraa_uart_data_available(uart, 10)){};
mraa_uart_read(uart, buffer, 1);
while(buffer[0] == '\n')
{
mraa_uart_read(uart, buffer, 1);
}
if(buffer[0] == '\0') direction = 'C';
mraa_uart_read(uart, buffer + 1, 1);
cross_clr = buffer[1];
}
printf("buff:%c %c %c \n",buffer[0], buffer[1],buffer[2]);
if(!sign){
if (direction == 'L') angle = CENTER - 0.005f;
if (direction == 'R') angle = CENTER + 0.005f;
if (direction == 'C') angle = CENTER;}
else
{
if (direction == 'C') angle = CENTER +0.013f;
if (direction == 'L') angle = CENTER +0.005f;
if (direction == 'R') angle = CENTER + 0.019f;
}
mraa_pwm_write(turn_pwm, angle);
speed_control(speed_pwm_in1, speed_pwm_in2, spd);
printf("speed: %d",spd);
usleep(250000);
direction = buffer[0];
cross_clr = buffer[1];
}
return 0;
}
/*!
* Direkter Zugriff auf den Motor
* @param left Geschwindigkeit fuer den linken Motor
* @param right Geschwindigkeit fuer den linken Motor
* Geschwindigkeit liegt zwischen -255 und +255.
* 0 bedeutet Stillstand, 255 volle Kraft voraus, -255 volle Kraft zurueck.
* Sinnvoll ist die Verwendung der Konstanten: BOT_SPEED_XXX,
* also z.B. motor_set(BOT_SPEED_LOW,-BOT_SPEED_LOW);
* fuer eine langsame Drehung
*/
void motor_set(int16 left, int16 right){
#ifdef SPEED_CONTROL_AVAILABLE
volatile static int16 old_mot_time_s=0;
volatile static int16 old_mot_time_ms=0;
#endif
if (left == BOT_SPEED_IGNORE)
left=BOT_SPEED_STOP;
if (right == BOT_SPEED_IGNORE)
right=BOT_SPEED_STOP;
// Haben wir ueberhaupt etwas zu tun?
if ((speed_l == left) && (speed_r == right)){
// Hier sitzt die eigentliche Regelung
#ifdef SPEED_CONTROL_AVAILABLE
if (timer_get_ms_since(old_mot_time_s,old_mot_time_ms) > SPEED_CONTROL_INTERVAL) {
speed_control();
old_mot_time_s= timer_get_s();
old_mot_time_ms= timer_get_ms();
}
#endif
return; // Keine Aenderung? Dann zuerueck
}
if (abs(left) > BOT_SPEED_MAX) // Nicht schneller fahren als moeglich
speed_l = BOT_SPEED_MAX;
else if (left == 0) // Stop wird nicht veraendert
speed_l = BOT_SPEED_STOP;
else if (abs(left) < BOT_SPEED_SLOW) // Nicht langsamer als die
speed_l = BOT_SPEED_SLOW; // Motoren koennen
else // Sonst den Wunsch uebernehmen
speed_l = abs(left);
if (abs(right) > BOT_SPEED_MAX)// Nicht schneller fahren als moeglich
speed_r = BOT_SPEED_MAX;
else if (abs(right) == 0) // Stop wird nicht veraendert
speed_r = BOT_SPEED_STOP;
else if (abs(right) < BOT_SPEED_SLOW) // Nicht langsamer als die
speed_r = BOT_SPEED_SLOW; // Motoren koennen
else // Sonst den Wunsch uebernehmen
speed_r = abs(right);
if (left < 0 )
speed_l=-speed_l;
if (right < 0 )
speed_r=-speed_r;
#ifdef SPEED_CONTROL_AVAILABLE
// TODO Hier koennten wir die Motorkennlinie heranziehen um gute Einstiegswerte fuer die Regelung zu haben
// Evtl. sogar eine im EEPROM kalibrierbare Tabelle??
encoderTargetRateL = left / SPEED_TO_ENCODER_RATE; // Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf]
encoderTargetRateR = right / SPEED_TO_ENCODER_RATE; // Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf]
#endif
// Zuerst einmal eine lineare Kennlinie annehmen
bot_motor(speed_l/2,speed_r/2);
}
void main(void)
{
int i1=0;
int count10S = 0;
Device_init();
disable_irq(PIT0_IRQn);
systick_delay_ms(50);
FLOAT_LDC_init(SPI1);
systick_delay_ms(50);
FLOAT_LDC_init(SPI0);
systick_delay_ms(1000);
uart_init(UART3,115200);
key_init(0);
key_init(1);
key_init(2);
//LDC_EVM_TEST();
//Speed_Ctl_Init(short point_value, double dKpp, double dKii, double dKdd);
while(1)
{
//value_collect();
LCD_P6x8Str(0,2,"Rmax Lmin");
printf("Rmax Rmin\r\n");
systick_delay_ms(1500);
i1=50;
while(i1--)
{
adjustment[2]=(float)filter(SPI1)/10;//右max
LCD_BL(55,2,(uint16)adjustment[2]);
printf("%d\r\n",adjustment[2]);
}
i1=50;
while(i1--)
{
adjustment[1]=(float)filter(SPI0)/10;//左min
LCD_BL(90,2,(uint16)adjustment[1]);
printf("%d\r\n",adjustment[1]);
}
LCD_BL(55,2,(uint16)adjustment[1]);
LCD_BL(90,2,(uint16)adjustment[2]);
LCD_P6x8Str(0,4,"Lmax Rmin");
printf("Lmax Rmin\r\n");
systick_delay_ms(1500);
i1=50;
while(i1--)
{
adjustment[3]=(float)filter(SPI1)/10;//右min
LCD_BL(55,4,(uint16)adjustment[3]);
printf("%d\r\n",adjustment[3]);
}
i1=50;
while(i1--)
{
adjustment[4]=(float)filter(SPI0)/10;//左max
LCD_BL(90,4,(uint16)adjustment[4]);
printf("%d\r\n",adjustment[4]);
}
LCD_BL(55,4,(uint16)adjustment[4]);
LCD_BL(90,4,(uint16)adjustment[3]);
if(adjustment[1]<=adjustment[3])
adjustment[5]=adjustment[1];//次小值
else
adjustment[5]=adjustment[3];//次小值
if(adjustment[2]>=adjustment[4])
adjustment[6]=adjustment[2];//次大值
else
adjustment[6]=adjustment[4];//次大值
divisor = (adjustment[2] - adjustment[3])/60; //48 //50
divisor2 = (adjustment[4] - adjustment[1])/59; //48 //45
zengfuzhi = (adjustment[2] - adjustment[3])/(adjustment[4] - adjustment[1]);
// for(int i=0;i<10;i++)
// {
// printf("%d\r\n",adjustment[i]);
// }
// systick_delay_ms(1000);
LCD_P6x8Str(0,6,"Mid value");
i1 = 50;
systick_delay_ms(1500);
while(i1--)
{
adjustment[7]=(float)filter(SPI0)/10;//左
adjustment[8]=(float)filter(SPI1)/10;//右
LCD_BL(55,6,(uint16)adjustment[7]);
LCD_BL(90,6,(uint16)adjustment[8]);
}
if(adjustment[7]>=adjustment[8])
{ flag=1;
adjustment[0]=adjustment[7]-adjustment[8];
}
else
{
flag=0;
adjustment[0]=adjustment[8]-adjustment[7];
}
LCD_Fill(0x00);
Out_side_collect();
LCD_Fill(0x00);
adjustment_change();
enable_irq(PIT0_IRQn);
while(1)
{
if(PIT_1sFlag == 1)
{
gpio_turn(PTD4);
PIT_1sFlag = 0;
count10S ++;
}
if(key_check(2) == 0)
{
LCD_Fill(0x00);
Motor_stop();
break;
}
else
{
if(PIT_5msFlag == 1)
{
LDC_get();
PIT_5msFlag = 0;
Steering_Change();
}
if(count10S > 10)
{
if(gpio_get(PTC4) == 0)
{
while(1)
{
Motor_stop();
Steering_Change();
}
}
}
if(PIT_10msFlag == 1)
{
Quad_count();
now = (float)guad_val;
Motor_PID(now);
PIT_10msFlag = 0;
}
if(PIT_20msFlag == 1)
{
PIT_20msFlag = 0;
}
if(PIT_50msFlag == 1)
{
//Result_collect();
PIT_50msFlag = 0;
}
//printf("guad_val:%d \r\n",(guad_val));
//TEST_display();
speed_control();
//key_choice();
uint16 i23 = gpio_get(PTC4);
//LCD_BL(50,2,(uint16)(i23));
}
}
}
}
int main() {
float speed, turn;
char speed_user_input[MAXBUFSIZ];
char turn_user_input[MAXBUFSIZ];
char time_user_input[MAXBUFSIZ];
mraa_pwm_context speed_pwm_in1, speed_pwm_in2, turn_pwm;
speed_pwm_in1 = mraa_pwm_init(3);
speed_pwm_in2 = mraa_pwm_init(5);
turn_pwm = mraa_pwm_init(6);
if (speed_pwm_in1 == NULL || speed_pwm_in2 == NULL || turn_pwm == NULL) {
fprintf(stderr, "Failed to initialized.\n");
return 1;
}
mraa_pwm_period_us(speed_pwm_in1,870); //1150Hz
mraa_pwm_enable(speed_pwm_in1, 1);
mraa_pwm_period_us(speed_pwm_in2,870);
mraa_pwm_enable(speed_pwm_in2, 1);
mraa_pwm_period_ms(turn_pwm,20);
mraa_pwm_enable(turn_pwm, 1);
mraa_pwm_write(turn_pwm, CENTER);
mraa_pwm_write(speed_pwm_in1, 1.0f);
mraa_pwm_write(speed_pwm_in2, 1.0f);
RearWheel *rw = new RearWheel();
//go for the curve
thread t1;
while(1) {
t1 = rw->threading();
printf("Turn (L, C, R): ");
scanf("%s", turn_user_input);
if (!strcmp(turn_user_input, "L") || !strcmp(turn_user_input, "l"))
turn = CENTER - 0.015f;
else if (!strcmp(turn_user_input, "C") || !strcmp(turn_user_input, "c"))
turn = CENTER;
else if (!strcmp(turn_user_input, "R") || !strcmp(turn_user_input, "r"))
turn = CENTER + 0.018f;
else {
printf("Wrong turn type!\n");
return 1;
}
printf("Speed value (-100-100): ");
scanf("%s", speed_user_input);
speed = atof(speed_user_input);
printf("time: ");
scanf("%s", time_user_input);
int time = atoi(time_user_input);
if (speed > 100 || speed < -100)
printf("Error: Choose between -100 and 100\n");
else {
mraa_pwm_write(turn_pwm, turn);
usleep(100000);
speed_control(speed_pwm_in1, speed_pwm_in2, speed);
}
sleep(time);
speed_control(speed_pwm_in1, speed_pwm_in2, 0.0f);
float count = rw->getCount();
printf("%.2f \n", count);
rw->clear();
t1.join();
}
return 0;
}
