Don't ad-block us - support your favorite websites. We have safe, unobstrusive, robotics related ads that you actually want to see - see here for more.
0 Members and 1 Guest are viewing this topic.
#include "mbed.h"Serial pc(USBTX, USBRX);Serial ax500(p9,p10);Timeout off;int a,b,c;void stop(){int b=0; ax500.printf("!a00\r"); wait_ms(10); ax500.printf("!b00\r"); wait_ms(1000); b=1; }void forward(unsigned char speed_A,unsigned char speed_B){char tick_A,tick_B,optical_A,optical_B; ax500.printf("!A%02x\r",speed_A); wait_ms(10); ax500.printf("!B%02x\r",speed_B); if (tick_A!=tickA){optical_A++;if(tickB!=tickB){optical_B++;}}int main() {int speed_A =60;int speed B =60;if speed A // from dec 0 --> 127(00-->7F) forward(speed); wait(5); }
#define PGain 10#define DGain 5#define IGain 1 //You play with these to make it work//Define global variablesint error = 0;int dEr = 0;int oldEr = 0;int integrateEr = 0;int SENSOR_LOC = 5; //5? Why not? Whatever you want it to beint PID_OUT = 0; //This is what you use to compute your control commandmain(){ ...do stuff, doesn't matter what. //READ SENSORS SENSOR_IN = readMySensor(); //this is whatever function you use to do it //update the errors error = SENSOR_LOC = SENSOR_IN; dEr = error - oldEr; //technically divide by dt, but this is handled by DGain integrateEr += error; //Add the error to the integrator oldEr = error; //vitally important, this step PID_OUT = PGain*error + DGain*dEr + IGain*integrateEr; //Now you set your limits. Let's say we're going to put PID_OUT directly //into the motors as a PWM command, but we need to limit it tbetween //0 and 127 if (PID_OUT < 0) PID_OUT = 0; if(PID_OUT > 127) PID_OUT = 127; //set your speed using your function SET_SPEED(PID_OUT);}
The main problem is I don't know how to make both motors drive straight.if I set speed to 60 for both, and If left motor is faster than right motor, then right motor speed++.
But then right motor speed would be faster,
so left motor have to ++.
QuoteBut then right motor speed would be faster,Faster than what? Faster than a target speed, or faster than too slow?Control each motor to get the encoder reported speed to match a target value. Then it will go as straight as it can given differences in wheel diameter, slip, etc. If one motor is too slow, speed it up. If the other motor is too fast, slow it down.
Quote from: jkerns on April 19, 2012, 08:14:46 PMQuoteBut then right motor speed would be faster,Faster than what? Faster than a target speed, or faster than too slow?Control each motor to get the encoder reported speed to match a target value. Then it will go as straight as it can given differences in wheel diameter, slip, etc. If one motor is too slow, speed it up. If the other motor is too fast, slow it down.Cause what I'm afraid is it will keep adding up until eventually the right motor will be faster than the leftmotor ,and then it will just snowball again and again until it reaches the max .How do I get it to match a target value when theres no target value?
#include "mbed.h"Serial pc(USBTX, USBRX);Serial ax500(p9,p10);InterruptIn trigger_A(p26);InterruptIn trigger_B(p25);Ticker calculate;//******************************************************************************************************************************************************************88//Define global variables.//Initialization.int enc_A,enc_B=0; char optical_A = 0;char optical_B = 0;int PGain=10;int DGain=5;int error = 0;int dEr = 0;int oldEr = 0;int integrateEr = 0;int SENSOR_LOC = 5; //5? Why not? Whatever you want it to beint PID_OUT = 0;int speed_B; unsigned int distance=0;//******************************************************************************************************************************************************************void calc(){error = enc_A - enc_B;dEr = error - oldEr; //technically divide by dt, but this is handled by DGainoldEr = error; //Copy error into oldErPID_OUT = PGain*error + DGain*dEr ;if(PID_OUT > 10) // Why 10? Okay I'll try 10 { PID_OUT = 10; }if(PID_OUT < -10) { PID_OUT = -10; } speed_B = speed_B + PID_OUT;}void stop(){ calculate.detach(); distance=(enc_A+enc_B)/2*20; //Total ticks from both enc/2 multiply with 20mm ax500.printf("!a00\r"); wait_ms(10); ax500.printf("!b00\r"); wait_ms(1000); enc_A=enc_B=0;}void forward(unsigned char speed_A,unsigned char speed_B){ if (speed_A>127) { speed_A=127; } if (speed_A<0) { speed_A=0; } else if (speed_B>127) { speed_B=127; } else if (speed_B<0) { speed_B=0; } ax500.printf("!A%02x\r",speed_A); wait_ms(10); ax500.printf("!B%02x\r",speed_B); calculate.attach(&calc,1.0); //Software interrupt. Calls calc function every 1 s.}void tick_A(){enc_A++;}void tick_B(){enc_B++;} int main() { // from dec 0 --> 127(00-->7F)int speed_A =60;int speed_B =60;trigger_A.rise(&tick_A);trigger_B.rise(&tick_B); forward(speed_A,speed_B); wait(4); stop(); wait(2); pc.printf("the distance is %u mm\r\n",distance); }