E2016-S1-Team5 / Mbed 2 deprecated Team5robotkode

Kildekode til robot

Dependencies:   PID mbed

Fork of Team5 by E2016-S1-Team5

robot.cpp@5:cf033e9d4468, 2016-11-24 (annotated)

Committer:
kimnielsen
Date:
Thu Nov 24 09:14:48 2016 +0000
Revision:
5:cf033e9d4468
Parent:
4:62a6681510d6
Child:
6:c2d4a02115db
h

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 5:cf033e9d4468 17 #include "PID.h"
kimnielsen 5:cf033e9d4468 18
kimnielsen 0:d3dbe632b1a9 19
kimnielsen 4:62a6681510d6 20 void startfunction( );
kimnielsen 0:d3dbe632b1a9 21 Ticker T1; // create an object T1 of Ticker
kimnielsen 0:d3dbe632b1a9 22
kimnielsen 2:1c27a43bb9b7 23 /*
kimnielsen 2:1c27a43bb9b7 24 ----------------------------------------------------------------------------
kimnielsen 4:62a6681510d6 25 MAIN FUNCTION
kimnielsen 4:62a6681510d6 26 ----------------------------------------------------------------------------*/
kimnielsen 4:62a6681510d6 27 int main()
kimnielsen 5:cf033e9d4468 28 {
kimnielsen 4:62a6681510d6 29 /*
kimnielsen 4:62a6681510d6 30 =============================================================================
kimnielsen 4:62a6681510d6 31 Driving and stopping the robot with member functions from the wheel class
kimnielsen 4:62a6681510d6 32 =============================================================================*/
kimnielsen 5:cf033e9d4468 33 startfunction(); // 1. DRIVE
kimnielsen 5:cf033e9d4468 34 get_to_goal();
kimnielsen 4:62a6681510d6 35
kimnielsen 4:62a6681510d6 36 robot.stop();
kimnielsen 4:62a6681510d6 37 printf("\n\rtimeused = %4d tickL= %4d tickR = %4d \n", t.read_ms(),tickL,
kimnielsen 4:62a6681510d6 38 tickR);
kimnielsen 4:62a6681510d6 39 wait_ms(3000);
kimnielsen 4:62a6681510d6 40 }
kimnielsen 4:62a6681510d6 41 /*
kimnielsen 2:1c27a43bb9b7 42 ----------------------------------------------------------------------------
kimnielsen 4:62a6681510d6 43 END OF MAIN FUNCTION
kimnielsen 4:62a6681510d6 44 ----------------------------------------------------------------------------*/
kimnielsen 4:62a6681510d6 45
kimnielsen 4:62a6681510d6 46 void startfunction()
kimnielsen 0:d3dbe632b1a9 47 {
kimnielsen 0:d3dbe632b1a9 48 T1.attach(&read_analog, 1.0); // attach the address of the read_analog
kimnielsen 0:d3dbe632b1a9 49 //function to read analog in every second
kimnielsen 0:d3dbe632b1a9 50
kimnielsen 0:d3dbe632b1a9 51 tacho_left.rise(&tickLeft); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 52 //to the falling edge
kimnielsen 0:d3dbe632b1a9 53 tacho_right.rise(&tickRight); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 54 //to the falling edge
kimnielsen 0:d3dbe632b1a9 55
kimnielsen 0:d3dbe632b1a9 56 HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
kimnielsen 0:d3dbe632b1a9 57 // (echo,trigger) on pin PC_5, PC6
kimnielsen 0:d3dbe632b1a9 58 Servo servo1(PC_8); //Create an object of Servo class on pin PC_8
kimnielsen 0:d3dbe632b1a9 59
kimnielsen 0:d3dbe632b1a9 60 sensor.setRanges(2, 400); // set the range for Ultrasonic
kimnielsen 0:d3dbe632b1a9 61
kimnielsen 2:1c27a43bb9b7 62 /*
kimnielsen 2:1c27a43bb9b7 63 =============================================================================
kimnielsen 2:1c27a43bb9b7 64 Demo of the servor and ulta sonic sensor
kimnielsen 4:62a6681510d6 65 =============================================================================*/
kimnielsen 0:d3dbe632b1a9 66 wait_ms(2000); // just get time for you to enable your screeen
kimnielsen 0:d3dbe632b1a9 67 servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
kimnielsen 0:d3dbe632b1a9 68 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 69 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the
kimnielsen 0:d3dbe632b1a9 70 //readings from ultra sonic at this position
kimnielsen 0:d3dbe632b1a9 71 servo1.set_position(180); // Servo left position (angle = 180 degree)
kimnielsen 0:d3dbe632b1a9 72 //for servo
kimnielsen 0:d3dbe632b1a9 73 wait_ms (500);
kimnielsen 2:1c27a43bb9b7 74 printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
kimnielsen 0:d3dbe632b1a9 75 servo1.set_position(90); // Straight ahead (angle = 90 degree) 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
kimnielsen 0:d3dbe632b1a9 79 init(); // initialise parameters (just for you to remember if you need to)
kimnielsen 0:d3dbe632b1a9 80
kimnielsen 0:d3dbe632b1a9 81 wait_ms(1000); //wait 1 secs here before you go
kimnielsen 0:d3dbe632b1a9 82 t.start(); // start timer (just demo of how you can use a timer)
kimnielsen 0:d3dbe632b1a9 83 }