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@2:74d8b693bc62, 2019-02-02 (annotated)

Committer:
Nicolaemf
Date:
Sat Feb 02 16:38:08 2019 +0000
Revision:
2:74d8b693bc62
Parent:
1:a2ceed49374e
Child:
3:54c41af9e119
Added comments to the main file, changed the pinout to a more coherent format;

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