Line follower robot with pid control

Archive
1

hello everyone

I tried to build line follower robot with my fez pandaII but I confused is there anyone who can help me
I implemented P control but it can not work at the high speed
here is my code



using System;
using System.Threading;
using GHIElectronics.NETMF.Hardware;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

using GHIElectronics.NETMF.FEZ;

namespace FEZ_Panda_II_Application1
{
    public class Program
    {
        static InputPort sensor1 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di35, false, Port.ResistorMode.PullUp);
        static InputPort sensor2 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di36, false, Port.ResistorMode.PullUp);
        static InputPort sensor3 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di38, false, Port.ResistorMode.PullUp);
        static InputPort sensor4 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di40, false, Port.ResistorMode.PullUp);
        static InputPort sensor5 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di42, false, Port.ResistorMode.PullUp);
        static InputPort sensor6 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di44, false, Port.ResistorMode.PullUp);
        static InputPort sensor7 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di46, false, Port.ResistorMode.PullUp);
        static InputPort sensor8 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di48, false, Port.ResistorMode.PullUp);
        static OutputPort led = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di52, true);

        static PWM leftpwm = new PWM((PWM.Pin)FEZ_Pin.PWM.Di8);
        static PWM rightpwm = new PWM((PWM.Pin)FEZ_Pin.PWM.Di10);


        public static int convert(bool value)
        {
            if (value == true)
                return 1;
            else
                return 0;

        }

        public static int read_sensor()
        {

            int sensor1_state = convert(sensor1.Read());
            int sensor2_state = convert(sensor2.Read());
            int sensor3_state = convert(sensor3.Read());
            int sensor4_state = convert(sensor4.Read());
            int sensor5_state = convert(sensor5.Read());
            int sensor6_state = convert(sensor6.Read());
            int sensor7_state = convert(sensor7.Read());
            int sensor8_state = convert(sensor8.Read());

              int error = sensor1_state * (-8) + sensor2_state * (-6) + sensor3_state * (-4) + sensor4_state * (-2)
                 + sensor5_state * 2 + sensor6_state * 4 + sensor7_state * 6 + sensor8_state * 8;

            return 0;

        }

        public static void motor(int error)
        {
            if (error >  100) error =  100;
            if (error < -100) error = -100;

            if (error < 0)
            {
                 leftpwm.Set(10000, (byte)(100));
                 rightpwm.Set(10000, (byte)(100 + error));
                 Debug.Print("right:" + (100 + error).ToString() + " left:100");
            }
            else
            {
                rightpwm.Set(10000, (byte)(100));
                leftpwm.Set(10000, (byte)(100 - error));
                Debug.Print("right:100 left:" + (100 - error).ToString());
            }
        }

        public static void Main()
        {
            led.Write(true);
       
            int Kp = 70;
            int ki = 0;
            int kd = 0;

            
            int last_error = 0;
            int error;
          //int proportional = 0;
            int integral = 0;
            int derivate = 0;

            int P;
            int I;
            int D;
            int PID;
            
            while (true)
            {

                error = read_sensor();
                P = error * Kp/10;

                integral += error;
                I = integral * ki/10;

                derivate = error - last_error;
                D = derivate * kd/10;

                last_error = error;

                PID = P + I + D;

                motor(PID);
                
            }
        }
    }
}
2

PIDs aren’t my thing (so this may be a silly question), but did you mean to always return 0 in your read_sensor() ??

3

no read_sensor is working correctly but when it start to turn it can not…I think because of speed

4

how can it respond to a turn when the error is always zero?

5

error is not always zero , for example when the robot is on the line if turning is so sharp robot is trying to turn but it can not…then it is going forward

6

Just curious … if you are always returning 0, why not make it a void method?

Also, all of the variables in read_sensor are local, so they all fall out of scope (including error). So that method seems like it effectively does nothing.

Were you expecting the error variable in the read_sensor variable to be available outside of that method?

7

error IS always zero. your method returns zero, you assign it to the error variable.

8

sorry it is actually written return error; but I was trying something and I forgot to correct it :smiley: I suprised why you all say same thing :smiley:

9

What do your debug statements show from your motor() method?