Janus Bo Andersen / Mbed 2 deprecated T2PRO1_master

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed m3pi

main.cpp@6:3be33adda32d, 2018-11-21 (annotated)

Committer:
janusboandersen
Date:
Wed Nov 21 11:58:56 2018 +0000
Revision:
6:3be33adda32d
Parent:
5:527c046c7b8a
Child:
7:30c63d6460f6
Print "Hej"

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:78f9794620a3 1 #include "mbed.h"
chris 0:78f9794620a3 2 #include "m3pi.h"
chris 0:78f9794620a3 3
chris 0:78f9794620a3 4 m3pi m3pi;
chris 0:78f9794620a3 5
chris 0:78f9794620a3 6 // Minimum and maximum motor speeds
Blunsdon 1:47ea51d9e25d 7 #define MAX 0.5
chris 0:78f9794620a3 8 #define MIN 0
chris 0:78f9794620a3 9
chris 0:78f9794620a3 10 // PID terms
chris 0:78f9794620a3 11 #define P_TERM 1
chris 0:78f9794620a3 12 #define I_TERM 0
Blunsdon 1:47ea51d9e25d 13 #define D_TERM 8
Blunsdon 1:47ea51d9e25d 14
Blunsdon 1:47ea51d9e25d 15 int sensor_all(int i) //lav en funktion? ved ikke hvad jeg skal her
Blunsdon 1:47ea51d9e25d 16 {
Blunsdon 1:47ea51d9e25d 17 int sensor_value[5]; // sensor int
Blunsdon 1:47ea51d9e25d 18 m3pi.putc(0x86); // send noget sensor data
Blunsdon 1:47ea51d9e25d 19
janusboandersen 4:5939f9ad88ca 20 for(int n=0; n < 5; n++) // forloop så sensor for data
Blunsdon 1:47ea51d9e25d 21 {
Blunsdon 1:47ea51d9e25d 22 char lowbyte = m3pi.getc();
Blunsdon 1:47ea51d9e25d 23 char hibyte = m3pi.getc();
Blunsdon 1:47ea51d9e25d 24 sensor_value[n] = ((lowbyte + (hibyte << 8))); // sensor value
Blunsdon 1:47ea51d9e25d 25 }
Blunsdon 1:47ea51d9e25d 26 return sensor_value[i]; //return sensor value string
Blunsdon 1:47ea51d9e25d 27 }
chris 0:78f9794620a3 28
chris 0:78f9794620a3 29 int main() {
janusboandersen 2:d22dcc2bfcc1 30
chris 0:78f9794620a3 31 m3pi.locate(0,1);
chris 0:78f9794620a3 32 m3pi.printf("Line PID");
chris 0:78f9794620a3 33
Blunsdon 1:47ea51d9e25d 34 wait(1.0);
chris 0:78f9794620a3 35
chris 0:78f9794620a3 36 m3pi.sensor_auto_calibrate();
chris 0:78f9794620a3 37
chris 0:78f9794620a3 38 float right;
chris 0:78f9794620a3 39 float left;
chris 0:78f9794620a3 40 float current_pos_of_line = 0.0;
chris 0:78f9794620a3 41 float previous_pos_of_line = 0.0;
chris 0:78f9794620a3 42 float derivative,proportional,integral = 0;
chris 0:78f9794620a3 43 float power;
chris 0:78f9794620a3 44 float speed = MAX;
chris 0:78f9794620a3 45
Blunsdon 1:47ea51d9e25d 46 int sensor_value0;
Blunsdon 1:47ea51d9e25d 47 int sensor_value4;
Blunsdon 1:47ea51d9e25d 48
chris 0:78f9794620a3 49 while (1) {
Blunsdon 1:47ea51d9e25d 50
chris 0:78f9794620a3 51 // Get the position of the line.
chris 0:78f9794620a3 52 current_pos_of_line = m3pi.line_position();
chris 0:78f9794620a3 53 proportional = current_pos_of_line;
chris 0:78f9794620a3 54
chris 0:78f9794620a3 55 // Compute the derivative
chris 0:78f9794620a3 56 derivative = current_pos_of_line - previous_pos_of_line;
chris 0:78f9794620a3 57
chris 0:78f9794620a3 58 // Compute the integral
chris 0:78f9794620a3 59 integral += proportional;
chris 0:78f9794620a3 60
chris 0:78f9794620a3 61 // Remember the last position.
chris 0:78f9794620a3 62 previous_pos_of_line = current_pos_of_line;
chris 0:78f9794620a3 63
chris 0:78f9794620a3 64 // Compute the power
chris 0:78f9794620a3 65 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
chris 0:78f9794620a3 66
chris 0:78f9794620a3 67 // Compute new speeds
chris 0:78f9794620a3 68 right = speed+power;
chris 0:78f9794620a3 69 left = speed-power;
chris 0:78f9794620a3 70
chris 0:78f9794620a3 71 // limit checks
chris 0:78f9794620a3 72 if (right < MIN)
chris 0:78f9794620a3 73 right = MIN;
chris 0:78f9794620a3 74 else if (right > MAX)
chris 0:78f9794620a3 75 right = MAX;
chris 0:78f9794620a3 76
chris 0:78f9794620a3 77 if (left < MIN)
chris 0:78f9794620a3 78 left = MIN;
chris 0:78f9794620a3 79 else if (left > MAX)
chris 0:78f9794620a3 80 left = MAX;
chris 0:78f9794620a3 81
chris 0:78f9794620a3 82 // set speed
chris 0:78f9794620a3 83 m3pi.left_motor(left);
chris 0:78f9794620a3 84 m3pi.right_motor(right);
Blunsdon 1:47ea51d9e25d 85
janusboandersen 2:d22dcc2bfcc1 86 //Marc's sensor test
janusboandersen 5:527c046c7b8a 87
janusboandersen 2:d22dcc2bfcc1 88 m3pi.cls();
janusboandersen 2:d22dcc2bfcc1 89 sensor_value0 = sensor_all(0); // sensor_value gets fkt value
janusboandersen 5:527c046c7b8a 90 /*
janusboandersen 2:d22dcc2bfcc1 91 sensor_value4 = sensor_all(4); // sensor_value gets fkt value
janusboandersen 6:3be33adda32d 92 */
janusboandersen 2:d22dcc2bfcc1 93 m3pi.locate(0,0);
janusboandersen 6:3be33adda32d 94 m3pi.printf("Hej");
janusboandersen 6:3be33adda32d 95
janusboandersen 6:3be33adda32d 96 /*
janusboandersen 2:d22dcc2bfcc1 97 m3pi.printf("%d", sensor_value0); // prints one of sidesensors value
janusboandersen 2:d22dcc2bfcc1 98 m3pi.locate(0,1);
janusboandersen 2:d22dcc2bfcc1 99 m3pi.printf("%d", sensor_value4); // prints the other sidesensor value
janusboandersen 3:47968be0df37 100 */
chris 0:78f9794620a3 101
chris 0:78f9794620a3 102 }
chris 0:78f9794620a3 103 }