Kildekode
Fork of Team5_kode by
Embed:
(wiki syntax)
Show/hide line numbers
robot.cpp
00001 /* 00002 ============================================================================ 00003 Name : robot.cpp 00004 Author : Team 5 00005 Version : 0.1 00006 Date : 13-10-2016 00007 Copyright : Open for all 00008 Description : Program to serve the platform for Pro1 2016 00009 ============================================================================ 00010 */ 00011 #include "odometry.h" 00012 #include "mbed.h" 00013 #include "HCSR04.h" 00014 #include "nucleo_servo.h" 00015 #include "hack_motor.h" 00016 #include <math.h> 00017 00018 void startfunction( ); 00019 Ticker T1; // create an object T1 of Ticker 00020 00021 /* 00022 ---------------------------------------------------------------------------- 00023 MAIN FUNCTION 00024 ----------------------------------------------------------------------------*/ 00025 int main() 00026 { 00027 /* 00028 ============================================================================= 00029 Driving and stopping the robot with member functions from the wheel class 00030 =============================================================================*/ 00031 startfunction(); // 1. DRIVE 00032 while(Dtravel_1() <= DISTANCES[0]) // OVERVEJ AT SMIDE "get_to_goal" funktionen ind i selve while loopet, sådan, at beregningerne sker parallelt. 00033 { 00034 get_to_goal(); // TJEK DETTE 00035 robot.FW(0.5, 0.5); 00036 wait_ms(5000); 00037 } 00038 00039 robot.stop(); 00040 printf("\n\rtimeused = %4d tickL= %4d tickR = %4d \n", t.read_ms(),tickL, 00041 tickR); 00042 wait_ms(3000); 00043 } 00044 /* 00045 ---------------------------------------------------------------------------- 00046 END OF MAIN FUNCTION 00047 ----------------------------------------------------------------------------*/ 00048 00049 void startfunction() 00050 { 00051 T1.attach(&read_analog, 1.0); // attach the address of the read_analog 00052 //function to read analog in every second 00053 00054 tacho_left.rise(&tickLeft); // attach the address of the count function 00055 //to the falling edge 00056 tacho_right.rise(&tickRight); // attach the address of the count function 00057 //to the falling edge 00058 00059 HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin 00060 // (echo,trigger) on pin PC_5, PC6 00061 Servo servo1(PC_8); //Create an object of Servo class on pin PC_8 00062 00063 sensor.setRanges(2, 400); // set the range for Ultrasonic 00064 00065 /* 00066 ============================================================================= 00067 Demo of the servor and ulta sonic sensor 00068 =============================================================================*/ 00069 wait_ms(2000); // just get time for you to enable your screeen 00070 servo1.set_position(0); // Servo right position (angle = 0 degree) for servo 00071 wait_ms (500); 00072 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the 00073 //readings from ultra sonic at this position 00074 servo1.set_position(180); // Servo left position (angle = 180 degree) 00075 //for servo 00076 wait_ms (500); 00077 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); 00078 servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo 00079 wait_ms (500); 00080 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); 00081 00082 init(); // initialise parameters (just for you to remember if you need to) 00083 00084 wait_ms(1000); //wait 1 secs here before you go 00085 t.start(); // start timer (just demo of how you can use a timer) 00086 }
Generated on Wed Jul 13 2022 02:32:05 by
1.7.2
