Important changes to forums and questions
All forums and questions are now archived. To start a new conversation or read the latest updates go to forums.mbed.com.
12 years, 6 months ago. This question has been closed. Reason: Duplicate of existing question.
Problems with PWM for sensors and motors
Hi
I am trying to implement a line following PID algorithm into my robot based on LPC1768. I am using PWM for motors and for LED intensity for sensors. However, I am experiencing problems with making both working at the same time. When the motor is on there are no readings from sensors. The pulses are both are the same. Could it the problem with my code? In my main I am assigning the return value from readSensors() function into s and then it is used to calculate PID.
The problem is I have no idea why I can't have both sensors and motors working at the same time. If you could give me any help it would be great. i know the code is big, but function readSensors works perfectly on its own same applies to motors.
#include "mbed.h"
//------MOTOR----------
I2C i2c(p9, p10); // sda, scl
PwmOut pwm0(p22);
PwmOut pwm1(p23);
const int addr = 0x40; // define the I2C Address
int fast = 7000; // fast speed
int medium = 6300; // Normal Speed Motor
int mediumFast = 5300; // Slow Speed Motor
int superslow = 8000;
float pid = 0;
//---Timer---
Timer timer;
//----COmmand Definition-----------
unsigned int FORWARD = 0x66;
unsigned int RIGHT = 0x55;
unsigned int LEFT = 0xAA;
//-----------Error for eacj position===========
int Sensor_Value0 = 0;
int Sensor_Value1 = 0;
int Sensor_Value2 = 0;
int Sensor_Value3 = 0;
int Sensor_Value4 = 0;
int Sensor_Value5 = 0;
int Sensor_Value6 = 0;
int Sensor_Value7 = 0;
//-------------------------------------------
float Sensor_High[8];
float Sensor_Low[8];
float Sensor_Value[8];
//Definition for positions
float HardRight = -0.5;
float Right = -0.25;
float Centre = 0;
float Left = 0.25;
float HardLeft = 0.5;
//-----Output for fuzzy logic----
//GLOBALS FOR PID
int position;
float targetPosition = 0; //desired setpoint
float error; //calculated error
float lastError = 0;
//-----------LINE SENSOR--------------------
DigitalOut led(LED1); // status LED
DigitalOut test(p27); // test pin
DigitalInOut ir_rx0(p11); // IR recieves
DigitalInOut ir_rx1(p12); // IR recieves
DigitalInOut ir_rx2(p15); // IR recieves
DigitalInOut ir_rx3(p16); // IR recieves
DigitalInOut ir_rx4(p17); // IR recieves
DigitalInOut ir_rx5(p18); // IR recieves
DigitalInOut ir_rx6(p25); // IR recieves
DigitalInOut ir_rx7(p26); // IR recieves
PwmOut ir_tx(p24); // IR transmitter
Serial pc(USBTX, USBRX); // USB serial port
// Black : 4.2x200us = 840us
// White : 2 x 100us = 200us
// Silver : 1.2 x 100us = 120us
float s;
float control;
float readSensors();
float PID(float output);
int main() {
char cmd[2];
pwm0.period_ms(20);
pwm1.period_ms(20);
pwm0.pulsewidth_us(superslow);
pwm1.pulsewidth_us(superslow);
while(1)
{
s = readSensors();
control = PID(s);
pwm0 = 0.35 -control; //left
pwm1 = 0.35 + pid;
cmd[0] = FORWARD;
cmd[1] = 0x00;
i2c.write(addr, cmd, 1);
}
}
float readSensors() {
float output;
int ir_value[8];
bool ir_set[8];
int ir_buffer;
int i, j, k;
ir_tx.period_ms(20); // a 20ms period
ir_tx.pulsewidth_ms(20); //
led = 1;
test = 0;
while(true)
{ //BASE Speed for motor
ir_rx0.output();
ir_rx1.output();
ir_rx2.output();
ir_rx3.output();
ir_rx4.output();
ir_rx5.output();
ir_rx6.output();
ir_rx7.output();
ir_rx0 = 1;
ir_rx1 = 1;
ir_rx2 = 1;
ir_rx3 = 1;
ir_rx4 = 1;
ir_rx5 = 1;
ir_rx6 = 1;
ir_rx7 = 1;
for(i=0; i<8; i++)
{
ir_set[i]=true;
ir_value[i]=0;
}
wait_us(50);
ir_rx0.input();
ir_rx1.input();
ir_rx2.input();
ir_rx3.input();
ir_rx4.input();
ir_rx5.input();
ir_rx6.input();
ir_rx7.input();
test = 1;
//Assigining a value from 0 to 20 to each sensor the minimum determines the strength
for(i=0; i<20; i++)
{
wait_us(50);
if ((ir_buffer && 0x01) == 0)
test = 0;
if((ir_rx0==0) & ir_set[0])
{
ir_set[0]=false; ir_value[0]=i;
}
if((ir_rx1==0) & ir_set[1])
{
ir_set[1]=false; ir_value[1]=i;
}
if((ir_rx2==0) & ir_set[2])
{
ir_set[2]=false; ir_value[2]=i;
}
if((ir_rx3==0) & ir_set[3])
{
ir_set[3]=false; ir_value[3]=i;
}
if((ir_rx4==0) & ir_set[4])
{
ir_set[4]=false; ir_value[4]=i;
}
if((ir_rx5==0) & ir_set[5])
{
ir_set[5]=false; ir_value[5]=i;
}
if((ir_rx6==0) & ir_set[6])
{
ir_set[6]=false; ir_value[6]=i;
}
if((ir_rx7==0) & ir_set[7])
{
ir_set[7]=false; ir_value[7]=i;
}
}
Sensor_Value[0] = ((15 - ir_value[0])*0.016666666667);
//printf( "SENSOR 0 VALUE = %f\n", Sensor_Value[0]);
if(Sensor_Value[0] < 0)
{
Sensor_Value[0] = 0;
}
else {
}
Sensor_Value[0] = ((15 - ir_value[0])*0.016666666667);
Sensor_Value[2] = ((14 - ir_value[2])*0.0125);
if(Sensor_Value[2] < 0)
{
Sensor_Value[2] = 0;
}
else {
Sensor_Value[2] = ((14 - ir_value[2])*0.0125);
}
printf( "SEN2 VAL:= %f\n", Sensor_Value[2]);
Sensor_Value[3] = (14 - ir_value[3])*0.020;
if(Sensor_Value[3] < 0)
{
Sensor_Value[3] = 0;
}
else {
Sensor_Value[3] = (14 - ir_value[3])*0.020;
}
printf( "SEN3 VAL:= %f\n", Sensor_Value[3]);
Sensor_Value[4]= (14 - ir_value[4])*0.020;
printf( "SEN4 VAL:= %f\n", Sensor_Value[4]);
Sensor_Value[5] = (14 - ir_value[5])*0.0125;
printf( "SEN5 VAL:= %f\n", Sensor_Value[5]);
Sensor_Value[7] = (14 - ir_value[7])*0.0125;
printf( "SEN7 VAL:= %f\n", Sensor_Value[7]);
output = (Sensor_Value[0]*HardRight)+(Sensor_Value[2]*Right)+(Sensor_Value[3]*Centre)+(Sensor_Value[5]*Left)+(Sensor_Value[7]*HardLeft);
output = output/(Sensor_Value[0] + Sensor_Value[2] + Sensor_Value[3] + Sensor_Value[5] + Sensor_Value[7]);
printf( "output = %f\n", output);
return output;
}
}
float PID(float output)
{
int begin, end;
//-------------------PID prototype---------------------
//-----------PD Constanst============
// Proportional constant
float KP = 0.7;
// Derivative constant
float KD = 0;
//Integral Constant
float KI = 0;
//the place where we will store our integral
int integral;
//the derative
int derivative = 0;
int timeDifference = 0;
//the place where we will store the last error value
timer.start();
begin = timer.read_us();
error = targetPosition - output;
//printf("DIFFERENCE BETWEEN ERRORS %d us", end - begin);
derivative = error - lastError;
//printf("DERIVATE %d\n", derivative);
integral = integral + error;
//pid formula
pid = KP*error + KI*integral + KD*derivative;
printf( "pid = %f\n", pid);
lastError = error;
end = timer.read_us();
timeDifference = end - begin;
return pid;
}