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@4:645b5d648c64, 2019-02-06 (annotated)

Committer:
Nicolaemf
Date:
Wed Feb 06 14:02:07 2019 +0000
Revision:
4:645b5d648c64
Parent:
3:54c41af9e119
Child:
5:bb0bec710c91
Removed RobotControl from IR sensor;

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 3:54c41af9e119 50 if(m_lineSensor[i]){
Nicolaemf 3:54c41af9e119 51 if(!count){
Nicolaemf 3:54c41af9e119 52 m_error = (i-2)*2;
Nicolaemf 3:54c41af9e119 53 count++;
Nicolaemf 3:54c41af9e119 54 }else if(count == 1){((m_error >= 0) ^ ((i-2)*2<0))? m_error++ : m_error = m_color;}
Nicolaemf 3:54c41af9e119 55 else if(count == 2){ ((m_error >= 0) ^ ((i-2)*2<0))? : m_error = m_color;}
Nicolaemf 3:54c41af9e119 56 else if(count<2){ printf("error : to many reading");}
Nicolaemf 3:54c41af9e119 57 }
Nicolaemf 3:54c41af9e119 58 }
Nicolaemf 3:54c41af9e119 59
Nicolaemf 3:54c41af9e119 60 if(!count){ m_prevError<0 ? m_error = -5 : m_error = 5;}
Nicolaemf 3:54c41af9e119 61
Nicolaemf 3:54c41af9e119 62 m_error = m_prevError;
Nicolaemf 1:a2ceed49374e 63 }
Nicolaemf 1:a2ceed49374e 64
Nicolaemf 1:a2ceed49374e 65
Nicolaemf 1:a2ceed49374e 66 void IRSensor::CalculatePID(){
Nicolaemf 1:a2ceed49374e 67 //as name suggests, calculates the proportions of corrections to be applied to the motors
Nicolaemf 1:a2ceed49374e 68 //error : error given by weightPID
Nicolaemf 1:a2ceed49374e 69 //*previousError : pointer to the previous error calculated by weightPID
Nicolaemf 1:a2ceed49374e 70 //returns the PID value
Nicolaemf 1:a2ceed49374e 71
Nicolaemf 1:a2ceed49374e 72 m_P = m_error;
Nicolaemf 1:a2ceed49374e 73 m_I = m_I + m_error;
Nicolaemf 1:a2ceed49374e 74 m_D = m_error - m_prevError;
Nicolaemf 1:a2ceed49374e 75
Nicolaemf 1:a2ceed49374e 76 m_prevError = m_error;
Nicolaemf 1:a2ceed49374e 77
Nicolaemf 1:a2ceed49374e 78 m_error = m_Kp*m_P + m_Ki*m_I + m_Kd*m_D;
Nicolaemf 1:a2ceed49374e 79 }
Nicolaemf 1:a2ceed49374e 80
Nicolaemf 1:a2ceed49374e 81
Nicolaemf 1:a2ceed49374e 82 void IRSensor::MotorControl(RobotControl controlLeft, RobotControl controlRight){
Nicolaemf 1:a2ceed49374e 83 //assigns the calculated direction to the motors
Nicolaemf 1:a2ceed49374e 84 //PIDvalue : calculated PID value given by CalculatePID
Nicolaemf 1:a2ceed49374e 85 //initSpeed : speed without correction
Nicolaemf 1:a2ceed49374e 86 //check previousSpeed to make sure the direction is the same
Nicolaemf 1:a2ceed49374e 87 float initSpeed = 0.2;
Nicolaemf 4:645b5d648c64 88 float error;
Nicolaemf 1:a2ceed49374e 89
Nicolaemf 1:a2ceed49374e 90 //scale and assign speed
Nicolaemf 4:645b5d648c64 91 error = m_error / 5.0f;
Nicolaemf 4:645b5d648c64 92 m_speedL = initSpeed - error;
Nicolaemf 4:645b5d648c64 93 m_speedR = initSpeed + error;
Nicolaemf 1:a2ceed49374e 94
Nicolaemf 4:645b5d648c64 95 m_speedL >= 0 ? m_dirL = true : m_dirR = false;
Nicolaemf 4:645b5d648c64 96 //if(m_dirL != m_prevDirL) controlLeft.SetDirection(m_dirL);
Nicolaemf 1:a2ceed49374e 97
Nicolaemf 4:645b5d648c64 98 m_speedR >= 0 ? m_dirL = true : m_dirR = false;
Nicolaemf 4:645b5d648c64 99 //if(m_dirR != m_prevDirR) controlRight.SetDirection(m_dirR);
Nicolaemf 1:a2ceed49374e 100
Nicolaemf 4:645b5d648c64 101 //controlLeft.SetSpeed(abs(m_speedL));
Nicolaemf 4:645b5d648c64 102 //controlRight.SetSpeed(abs(m_speedR));
Nicolaemf 1:a2ceed49374e 103
Nicolaemf 4:645b5d648c64 104 //m_prevDirL = m_dirL;
Nicolaemf 4:645b5d648c64 105 //m_prevDirR = m_dirR;
Nicolaemf 4:645b5d648c64 106
Nicolaemf 4:645b5d648c64 107 //printf("leftSpeed : %f, rightSpeed %f", m_speedL, m_speedR);
Nicolaemf 1:a2ceed49374e 108
Nicolaemf 1:a2ceed49374e 109
Nicolaemf 1:a2ceed49374e 110
Nicolaemf 1:a2ceed49374e 111 }