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@1:a2ceed49374e, 2019-02-02 (annotated)

Committer:
Nicolaemf
Date:
Sat Feb 02 16:11:37 2019 +0000
Revision:
1:a2ceed49374e
Child:
2:74d8b693bc62
implemented IRsensor class and added all the needed methods except weightPID

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 1:a2ceed49374e 7 //class constructor
Nicolaemf 1:a2ceed49374e 8 m_P = 0.0;
Nicolaemf 1:a2ceed49374e 9 m_I = 0.0;
Nicolaemf 1:a2ceed49374e 10 m_D = 0.0;
Nicolaemf 1:a2ceed49374e 11 m_toggle = false;
Nicolaemf 1:a2ceed49374e 12 Sample();
Nicolaemf 1:a2ceed49374e 13
Nicolaemf 1:a2ceed49374e 14 }
Nicolaemf 1:a2ceed49374e 15
Nicolaemf 1:a2ceed49374e 16
Nicolaemf 1:a2ceed49374e 17 void IRSensor::Sample(){
Nicolaemf 1:a2ceed49374e 18 //function attached to the ticker
Nicolaemf 1:a2ceed49374e 19 //assigns the data recieved for each digital in into an array
Nicolaemf 1:a2ceed49374e 20 //toggle is toggled at every ISR, signifying the ISR has occured
Nicolaemf 1:a2ceed49374e 21
Nicolaemf 1:a2ceed49374e 22 m_lineSensor[0] = m_leftIR;
Nicolaemf 1:a2ceed49374e 23 m_lineSensor[1] = m_midLeftIR;
Nicolaemf 1:a2ceed49374e 24 m_lineSensor[2] = m_midIR;
Nicolaemf 1:a2ceed49374e 25 m_lineSensor[3] = m_midRightIR;
Nicolaemf 1:a2ceed49374e 26 m_lineSensor[4] = m_rightIR;
Nicolaemf 1:a2ceed49374e 27
Nicolaemf 1:a2ceed49374e 28 m_toggle = m_toggle;
Nicolaemf 1:a2ceed49374e 29 }
Nicolaemf 1:a2ceed49374e 30
Nicolaemf 1:a2ceed49374e 31 void IRSensor::WeightPID(){
Nicolaemf 1:a2ceed49374e 32
Nicolaemf 1:a2ceed49374e 33 }
Nicolaemf 1:a2ceed49374e 34
Nicolaemf 1:a2ceed49374e 35
Nicolaemf 1:a2ceed49374e 36 void IRSensor::CalculatePID(){
Nicolaemf 1:a2ceed49374e 37 //as name suggests, calculates the proportions of corrections to be applied to the motors
Nicolaemf 1:a2ceed49374e 38 //error : error given by weightPID
Nicolaemf 1:a2ceed49374e 39 //*previousError : pointer to the previous error calculated by weightPID
Nicolaemf 1:a2ceed49374e 40 //returns the PID value
Nicolaemf 1:a2ceed49374e 41
Nicolaemf 1:a2ceed49374e 42 m_P = m_error;
Nicolaemf 1:a2ceed49374e 43 m_I = m_I + m_error;
Nicolaemf 1:a2ceed49374e 44 m_D = m_error - m_prevError;
Nicolaemf 1:a2ceed49374e 45
Nicolaemf 1:a2ceed49374e 46 m_prevError = m_error;
Nicolaemf 1:a2ceed49374e 47
Nicolaemf 1:a2ceed49374e 48 m_error = m_Kp*m_P + m_Ki*m_I + m_Kd*m_D;
Nicolaemf 1:a2ceed49374e 49 }
Nicolaemf 1:a2ceed49374e 50
Nicolaemf 1:a2ceed49374e 51
Nicolaemf 1:a2ceed49374e 52 void IRSensor::MotorControl(RobotControl controlLeft, RobotControl controlRight){
Nicolaemf 1:a2ceed49374e 53 //assigns the calculated direction to the motors
Nicolaemf 1:a2ceed49374e 54 //PIDvalue : calculated PID value given by CalculatePID
Nicolaemf 1:a2ceed49374e 55 //initSpeed : speed without correction
Nicolaemf 1:a2ceed49374e 56 //check previousSpeed to make sure the direction is the same
Nicolaemf 1:a2ceed49374e 57 float initSpeed = 0.2;
Nicolaemf 1:a2ceed49374e 58
Nicolaemf 1:a2ceed49374e 59
Nicolaemf 1:a2ceed49374e 60 //scale and assign speed
Nicolaemf 1:a2ceed49374e 61 m_error = m_error / 5;
Nicolaemf 1:a2ceed49374e 62 m_speedL = initSpeed - m_error;
Nicolaemf 1:a2ceed49374e 63 m_speedR = initSpeed + m_error;
Nicolaemf 1:a2ceed49374e 64
Nicolaemf 1:a2ceed49374e 65 m_speedL > 0 ? m_dirL = true : m_dirR = false;
Nicolaemf 1:a2ceed49374e 66 if(m_dirL != m_prevDirL) controlLeft.SetDirection(m_dirL);
Nicolaemf 1:a2ceed49374e 67
Nicolaemf 1:a2ceed49374e 68 m_speedR > 0 ? m_dirL = true : m_dirR = false;
Nicolaemf 1:a2ceed49374e 69 if(m_dirR != m_prevDirR) controlRight.SetDirection(m_dirR);
Nicolaemf 1:a2ceed49374e 70
Nicolaemf 1:a2ceed49374e 71 controlLeft.SetSpeed(m_speedL);
Nicolaemf 1:a2ceed49374e 72 controlRight.SetSpeed(m_speedR);
Nicolaemf 1:a2ceed49374e 73
Nicolaemf 1:a2ceed49374e 74 m_prevDirL = m_dirL;
Nicolaemf 1:a2ceed49374e 75 m_prevDirR = m_dirR;
Nicolaemf 1:a2ceed49374e 76
Nicolaemf 1:a2ceed49374e 77
Nicolaemf 1:a2ceed49374e 78
Nicolaemf 1:a2ceed49374e 79 }