Nicolae Marton / Mbed 2 deprecated TDP3_OOP

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

LineFollowingRobot/IRsensor.cpp@5:bb0bec710c91, 2019-03-07 (annotated)

Committer:
Nicolaemf
Date:
Thu Mar 07 14:00:42 2019 +0000
Revision:
5:bb0bec710c91
Parent:
4:645b5d648c64 
linefollower;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nicolaemf 1:a2ceed49374e 1 #include <mbed.h>
Nicolaemf 1:a2ceed49374e 2 #include "IRSensor_H.h"
Nicolaemf 1:a2ceed49374e 3 #include "RobotControl_H.h"
Nicolaemf 1:a2ceed49374e 4
Nicolaemf 1:a2ceed49374e 5 IRSensor::IRSensor(PinName pin1,PinName pin2,PinName pin3,PinName pin4,PinName pin5, float Kp, float Ki, float Kd):
Nicolaemf 1:a2ceed49374e 6 m_leftIR(pin1), m_midLeftIR(pin2), m_midIR(pin3), m_midRightIR(pin4), m_rightIR(pin5), m_Kp(Kp), m_Ki(Ki), m_Kd(Kd){
Nicolaemf 3:54c41af9e119 7 //class constructor, initialises pins and variables and gets a first reading on the sensors
Nicolaemf 2:74d8b693bc62 8
Nicolaemf 1:a2ceed49374e 9 m_P = 0.0;
Nicolaemf 1:a2ceed49374e 10 m_I = 0.0;
Nicolaemf 1:a2ceed49374e 11 m_D = 0.0;
Nicolaemf 2:74d8b693bc62 12
Nicolaemf 1:a2ceed49374e 13 m_toggle = false;
Nicolaemf 3:54c41af9e119 14
Nicolaemf 2:74d8b693bc62 15 m_dirL = true;
Nicolaemf 2:74d8b693bc62 16 m_dirR = true;
Nicolaemf 2:74d8b693bc62 17
Nicolaemf 3:54c41af9e119 18 m_prevDirL = true;
Nicolaemf 3:54c41af9e119 19 m_prevDirR = true;
Nicolaemf 3:54c41af9e119 20
Nicolaemf 3:54c41af9e119 21 m_color = 5;
Nicolaemf 3:54c41af9e119 22
Nicolaemf 1:a2ceed49374e 23 Sample();
Nicolaemf 1:a2ceed49374e 24
Nicolaemf 1:a2ceed49374e 25 }
Nicolaemf 1:a2ceed49374e 26
Nicolaemf 1:a2ceed49374e 27
Nicolaemf 1:a2ceed49374e 28 void IRSensor::Sample(){
Nicolaemf 1:a2ceed49374e 29 //function attached to the ticker
Nicolaemf 1:a2ceed49374e 30 //assigns the data recieved for each digital in into an array
Nicolaemf 1:a2ceed49374e 31 //toggle is toggled at every ISR, signifying the ISR has occured
Nicolaemf 1:a2ceed49374e 32
Nicolaemf 1:a2ceed49374e 33 m_lineSensor[0] = m_leftIR;
Nicolaemf 1:a2ceed49374e 34 m_lineSensor[1] = m_midLeftIR;
Nicolaemf 1:a2ceed49374e 35 m_lineSensor[2] = m_midIR;
Nicolaemf 1:a2ceed49374e 36 m_lineSensor[3] = m_midRightIR;
Nicolaemf 1:a2ceed49374e 37 m_lineSensor[4] = m_rightIR;
Nicolaemf 1:a2ceed49374e 38
Nicolaemf 4:645b5d648c64 39 m_toggle = !m_toggle;
Nicolaemf 1:a2ceed49374e 40 }
Nicolaemf 1:a2ceed49374e 41
Nicolaemf 3:54c41af9e119 42
Nicolaemf 1:a2ceed49374e 43 void IRSensor::WeightPID(){
Nicolaemf 1:a2ceed49374e 44
Nicolaemf 3:54c41af9e119 45 int i;
Nicolaemf 3:54c41af9e119 46 int count = 0;
Nicolaemf 3:54c41af9e119 47
Nicolaemf 3:54c41af9e119 48 for(i = 0; i <5; i++){
Nicolaemf 3:54c41af9e119 49
Nicolaemf 5:bb0bec710c91 50
Nicolaemf 5:bb0bec710c91 51 if(!m_lineSensor[i]){
Nicolaemf 5:bb0bec710c91 52
Nicolaemf 3:54c41af9e119 53 if(!count){
Nicolaemf 3:54c41af9e119 54 m_error = (i-2)*2;
Nicolaemf 3:54c41af9e119 55 count++;
Nicolaemf 5:bb0bec710c91 56
Nicolaemf 5:bb0bec710c91 57 }else if(count == 1){
Nicolaemf 5:bb0bec710c91 58 ((m_error >= 0) ^ ((i-2)*2<0))||((m_error <= 0) ^ ((i-2)*2>0)) ? m_error++ : m_error = m_color;
Nicolaemf 5:bb0bec710c91 59 count++;
Nicolaemf 5:bb0bec710c91 60 }
Nicolaemf 5:bb0bec710c91 61 else if(count == 2){
Nicolaemf 5:bb0bec710c91 62 ((m_error >= 0) ^ ((i-2)*2<0))? : m_error = m_color;
Nicolaemf 5:bb0bec710c91 63 count++;
Nicolaemf 5:bb0bec710c91 64 }
Nicolaemf 5:bb0bec710c91 65 else if(count>2){ printf("error : to many reading");}
Nicolaemf 5:bb0bec710c91 66
Nicolaemf 3:54c41af9e119 67 }
Nicolaemf 3:54c41af9e119 68 }
Nicolaemf 5:bb0bec710c91 69
Nicolaemf 3:54c41af9e119 70 if(!count){ m_prevError<0 ? m_error = -5 : m_error = 5;}
Nicolaemf 1:a2ceed49374e 71 }
Nicolaemf 1:a2ceed49374e 72
Nicolaemf 1:a2ceed49374e 73
Nicolaemf 1:a2ceed49374e 74 void IRSensor::CalculatePID(){
Nicolaemf 1:a2ceed49374e 75 //as name suggests, calculates the proportions of corrections to be applied to the motors
Nicolaemf 1:a2ceed49374e 76 //error : error given by weightPID
Nicolaemf 1:a2ceed49374e 77 //*previousError : pointer to the previous error calculated by weightPID
Nicolaemf 1:a2ceed49374e 78 //returns the PID value
Nicolaemf 1:a2ceed49374e 79
Nicolaemf 5:bb0bec710c91 80 m_P = m_error*1.0;
Nicolaemf 5:bb0bec710c91 81 m_I = (m_I + m_error)*1.0;
Nicolaemf 5:bb0bec710c91 82 m_D = 1.0*(m_error - m_prevError);
Nicolaemf 1:a2ceed49374e 83
Nicolaemf 1:a2ceed49374e 84 m_prevError = m_error;
Nicolaemf 1:a2ceed49374e 85
Nicolaemf 5:bb0bec710c91 86 m_PID = m_Kp*m_P + m_Ki*m_I + m_Kd*m_D;
Nicolaemf 5:bb0bec710c91 87 //printf("error is : %i , Kp is : %f , P is : %f \r",m_error, m_Kp, m_P);
Nicolaemf 5:bb0bec710c91 88
Nicolaemf 1:a2ceed49374e 89 }
Nicolaemf 1:a2ceed49374e 90
Nicolaemf 1:a2ceed49374e 91
Nicolaemf 5:bb0bec710c91 92 void IRSensor::MotorControl(){
Nicolaemf 1:a2ceed49374e 93 //assigns the calculated direction to the motors
Nicolaemf 1:a2ceed49374e 94 //PIDvalue : calculated PID value given by CalculatePID
Nicolaemf 1:a2ceed49374e 95 //initSpeed : speed without correction
Nicolaemf 1:a2ceed49374e 96 //check previousSpeed to make sure the direction is the same
Nicolaemf 5:bb0bec710c91 97 float initSpeed = 0.21;
Nicolaemf 4:645b5d648c64 98 float error;
Nicolaemf 1:a2ceed49374e 99
Nicolaemf 1:a2ceed49374e 100 //scale and assign speed
Nicolaemf 5:bb0bec710c91 101 error = m_PID / 5.0;
Nicolaemf 5:bb0bec710c91 102
Nicolaemf 5:bb0bec710c91 103 //printf("m_error : %f \t error : %f \r", m_error, error);
Nicolaemf 4:645b5d648c64 104 m_speedL = initSpeed - error;
Nicolaemf 4:645b5d648c64 105 m_speedR = initSpeed + error;
Nicolaemf 1:a2ceed49374e 106
Nicolaemf 5:bb0bec710c91 107 m_speedL >= 0 ? m_dirL = true : m_dirL = false;
Nicolaemf 4:645b5d648c64 108 //if(m_dirL != m_prevDirL) controlLeft.SetDirection(m_dirL);
Nicolaemf 1:a2ceed49374e 109
Nicolaemf 5:bb0bec710c91 110 m_speedR >= 0 ? m_dirR = true : m_dirR = false;
Nicolaemf 4:645b5d648c64 111 //if(m_dirR != m_prevDirR) controlRight.SetDirection(m_dirR);
Nicolaemf 1:a2ceed49374e 112
Nicolaemf 5:bb0bec710c91 113 printf("leftspeed is : %f, rightspeed is : %f \r", m_speedL, m_speedR);
Nicolaemf 1:a2ceed49374e 114
Nicolaemf 1:a2ceed49374e 115
Nicolaemf 1:a2ceed49374e 116 }