Kim Nielsen / Mbed 2 deprecated Endeligkildekode

Endelig kildekode med rettelser.

Dependencies:   PID mbed

Fork of EndeligKildekode by E2016-S1-Team5

robot.cpp@4:62a6681510d6, 2016-11-23 (annotated)

Committer:
kimnielsen
Date:
Wed Nov 23 21:40:26 2016 +0000
Revision:
4:62a6681510d6
Parent:
3:30bdc3d9e1c6
Child:
5:cf033e9d4468
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kimnielsen 0:d3dbe632b1a9 1 /*
kimnielsen 0:d3dbe632b1a9 2 ============================================================================
kimnielsen 0:d3dbe632b1a9 3 Name : robot.cpp
kimnielsen 2:1c27a43bb9b7 4 Author : Team 5
kimnielsen 0:d3dbe632b1a9 5 Version : 0.1
kimnielsen 2:1c27a43bb9b7 6 Date : 13-10-2016
kimnielsen 0:d3dbe632b1a9 7 Copyright : Open for all
kimnielsen 4:62a6681510d6 8 Description : Program to serve the platform for Pro1 2016
kimnielsen 0:d3dbe632b1a9 9 ============================================================================
kimnielsen 0:d3dbe632b1a9 10 */
kimnielsen 4:62a6681510d6 11 #include "odometry.h"
kimnielsen 0:d3dbe632b1a9 12 #include "mbed.h"
kimnielsen 0:d3dbe632b1a9 13 #include "HCSR04.h"
kimnielsen 0:d3dbe632b1a9 14 #include "nucleo_servo.h"
kimnielsen 0:d3dbe632b1a9 15 #include "hack_motor.h"
kimnielsen 2:1c27a43bb9b7 16 #include <math.h>
kimnielsen 0:d3dbe632b1a9 17
kimnielsen 4:62a6681510d6 18 void startfunction( );
kimnielsen 0:d3dbe632b1a9 19 Ticker T1; // create an object T1 of Ticker
kimnielsen 0:d3dbe632b1a9 20
kimnielsen 2:1c27a43bb9b7 21 /*
kimnielsen 2:1c27a43bb9b7 22 ----------------------------------------------------------------------------
kimnielsen 4:62a6681510d6 23 MAIN FUNCTION
kimnielsen 4:62a6681510d6 24 ----------------------------------------------------------------------------*/
kimnielsen 4:62a6681510d6 25 int main()
kimnielsen 4:62a6681510d6 26 {
kimnielsen 4:62a6681510d6 27 /*
kimnielsen 4:62a6681510d6 28 =============================================================================
kimnielsen 4:62a6681510d6 29 Driving and stopping the robot with member functions from the wheel class
kimnielsen 4:62a6681510d6 30 =============================================================================*/
kimnielsen 4:62a6681510d6 31 startfunction(); // 1. DRIVE
kimnielsen 4:62a6681510d6 32 while(Dtravel_1() <= DISTANCES[0]) // OVERVEJ AT SMIDE "get_to_goal" funktionen ind i selve while loopet, sådan, at beregningerne sker parallelt.
kimnielsen 4:62a6681510d6 33 {
kimnielsen 4:62a6681510d6 34 get_to_goal(); // TJEK DETTE
kimnielsen 4:62a6681510d6 35 robot.FW(0.5, 0.5);
kimnielsen 4:62a6681510d6 36 wait_ms(5000);
kimnielsen 4:62a6681510d6 37 }
kimnielsen 4:62a6681510d6 38
kimnielsen 4:62a6681510d6 39 robot.stop();
kimnielsen 4:62a6681510d6 40 printf("\n\rtimeused = %4d tickL= %4d tickR = %4d \n", t.read_ms(),tickL,
kimnielsen 4:62a6681510d6 41 tickR);
kimnielsen 4:62a6681510d6 42 wait_ms(3000);
kimnielsen 4:62a6681510d6 43 }
kimnielsen 4:62a6681510d6 44 /*
kimnielsen 2:1c27a43bb9b7 45 ----------------------------------------------------------------------------
kimnielsen 4:62a6681510d6 46 END OF MAIN FUNCTION
kimnielsen 4:62a6681510d6 47 ----------------------------------------------------------------------------*/
kimnielsen 4:62a6681510d6 48
kimnielsen 4:62a6681510d6 49 void startfunction()
kimnielsen 0:d3dbe632b1a9 50 {
kimnielsen 0:d3dbe632b1a9 51 T1.attach(&read_analog, 1.0); // attach the address of the read_analog
kimnielsen 0:d3dbe632b1a9 52 //function to read analog in every second
kimnielsen 0:d3dbe632b1a9 53
kimnielsen 0:d3dbe632b1a9 54 tacho_left.rise(&tickLeft); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 55 //to the falling edge
kimnielsen 0:d3dbe632b1a9 56 tacho_right.rise(&tickRight); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 57 //to the falling edge
kimnielsen 0:d3dbe632b1a9 58
kimnielsen 0:d3dbe632b1a9 59 HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
kimnielsen 0:d3dbe632b1a9 60 // (echo,trigger) on pin PC_5, PC6
kimnielsen 0:d3dbe632b1a9 61 Servo servo1(PC_8); //Create an object of Servo class on pin PC_8
kimnielsen 0:d3dbe632b1a9 62
kimnielsen 0:d3dbe632b1a9 63 sensor.setRanges(2, 400); // set the range for Ultrasonic
kimnielsen 0:d3dbe632b1a9 64
kimnielsen 2:1c27a43bb9b7 65 /*
kimnielsen 2:1c27a43bb9b7 66 =============================================================================
kimnielsen 2:1c27a43bb9b7 67 Demo of the servor and ulta sonic sensor
kimnielsen 4:62a6681510d6 68 =============================================================================*/
kimnielsen 0:d3dbe632b1a9 69 wait_ms(2000); // just get time for you to enable your screeen
kimnielsen 0:d3dbe632b1a9 70 servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
kimnielsen 0:d3dbe632b1a9 71 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 72 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the
kimnielsen 0:d3dbe632b1a9 73 //readings from ultra sonic at this position
kimnielsen 0:d3dbe632b1a9 74 servo1.set_position(180); // Servo left position (angle = 180 degree)
kimnielsen 0:d3dbe632b1a9 75 //for servo
kimnielsen 0:d3dbe632b1a9 76 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 77 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
kimnielsen 0:d3dbe632b1a9 78 servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
kimnielsen 0:d3dbe632b1a9 79 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 80 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
kimnielsen 0:d3dbe632b1a9 81
kimnielsen 0:d3dbe632b1a9 82 init(); // initialise parameters (just for you to remember if you need to)
kimnielsen 0:d3dbe632b1a9 83
kimnielsen 0:d3dbe632b1a9 84 wait_ms(1000); //wait 1 secs here before you go
kimnielsen 0:d3dbe632b1a9 85 t.start(); // start timer (just demo of how you can use a timer)
kimnielsen 0:d3dbe632b1a9 86 }