Kildekode

Dependencies:   mbed

Fork of Team5_kode by E2016-S1-Team5

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers robot.cpp Source File

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 }