Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <project.h>
- #include <stdio.h>
- #include "FreeRTOS.h"
- #include "task.h"
- #include "Motor.h"
- #include "Ultra.h"
- #include "Nunchuk.h"
- #include "Reflectance.h"
- #include "Gyro.h"
- #include "Accel_magnet.h"
- #include "LSM303D.h"
- #include "IR.h"
- #include "Beep.h"
- #include "mqtt_sender.h"
- #include <time.h>
- #include <sys/time.h>
- #include "serial1.h"
- #include <unistd.h>
- #if 1
- void motor_forward_fixed(uint8 speed, uint32 delay)
- {
- MotorDirLeft_Write(0);
- MotorDirRight_Write(0);
- float ratio = 1.05; // pitää säätää, 1.3 vähän liikaa
- uint8 l_speed = speed;
- uint8 r_speed = (float)speed / ratio;
- printf("l_speed: %d, r_speed: %d\n", l_speed, r_speed); // tulostaa nopeudet
- motor_turn(l_speed, r_speed, delay);
- }
- void turn_left()
- {
- motor_turn(20, 70, 200);
- }
- void turn_right()
- {
- motor_turn(70, 20, 200);
- }
- void mild_turn_left()
- {
- motor_turn(30, 70, 100);
- }
- void mild_turn_right()
- {
- motor_turn(70, 30, 100);
- }
- void hard_turn_left()
- {
- motor_turn(0, 70, 500);
- }
- void hard_turn_right()
- {
- motor_turn(70, 0, 500);
- }
- void zmain(void)
- {
- IR_Start(); //käynnistetään IR sensori
- IR_flush();
- reflectance_start(); //Käynnistetään reflectance sensorit
- //struct sensors_ ref;
- struct sensors_ dig;
- reflectance_set_threshold(9000, 9000, 11000, 11000, 9000, 9000);
- motor_start(); //käynnistetään moottori
- motor_forward(0, 0); //pysäytetään moottorit
- int linjalaskuri = 0;
- bool led = false; //ledi päälle kun robotti saa IR arvon
- while (SW1_Read()==1)
- {
- vTaskDelay(200);
- }
- for(;;)
- {
- reflectance_digital(&dig); // dig.l3, dig.l2, dig.l1, dig.r1, dig.r2, dig.r3
- motor_forward_fixed(70,0);
- // VIIVAN SEURANTA
- if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0)
- {
- // mene suoraan
- motor_forward_fixed(70, 0);
- }
- else if (dig.l3 == 0 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
- {
- // käänny oikea
- turn_right();
- }
- else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 0)
- {
- // käänny vasen
- turn_left();
- }
- else if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
- {
- // käänny vahva oikea
- hard_turn_right();
- }
- else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 1 && dig.r3 == 1)
- {
- // käänny vahva vasen
- hard_turn_right();
- }
- else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0)
- {
- // käänny loiva vasen
- mild_turn_left();
- }
- else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
- {
- // käänny loiva oikea
- mild_turn_right();
- }
- else if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1)
- {
- // kaikki on ykkösiä
- if(linjalaskuri==0)
- {
- motor_forward(0,0);
- IR_wait();
- led=!led;
- linjalaskuri++;
- }
- else if(linjalaskuri<=4)
- {
- motor_forward_fixed(100,0);
- linjalaskuri++;
- }
- else
- {
- motor_forward(0,0);
- }
- // odota IR tai linjalaskuri++;
- }
- else
- {
- motor_forward_fixed(70,0);
- }
- // lisää iffejä...
- vTaskDelay(50);
- }
- }
- #endif
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement