Hello guys, I`m making some kind of robot, which I would like to control with my phone. I made somean android app, which runs fine, now I need only one thing. I need it to fully control my robot. I would like to make a two way robot: one is controlled with move arrows (manual), and other automatic (wall follower) .
I made robot to be controlled manual. Arrows send different letter, which tells to robot to move somewhere (F - Forward, B - Backward and so on). It works just fine. Now I want to be able to switch between programs. What I`m trying to do, is when I send to robot letter 'G' I can control manual, and when I send 'C', It will start doing the main program. What makes it hard, my whole program is kind of long, multiple statements in loop, and when I try to get everything together with swich(c) and case 'C" it just does not work fine. Anyone can help with this?
I will upload my main code with loop and setup statements, which does not work.
#include <AFMotor.h>
#include <PID_v1.h>
#include "const.h"
#include "debug.h"
#include "drive.h"
#include "sensor.h"
#include <NewPing.h>
#include <SoftwareSerial.h> //the library for seial communication
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
NewPing sonar[SONAR_NUM] = { // Sensor object array.
NewPing(A0, A1, MAX_DISTANCE),
NewPing(A2, A3, MAX_DISTANCE),
};
NewPing sonar_top(A4, A5, MAX_DISTANCE);
/*
void counter()
{
Update count
pulses++;
}
*/
void setup()
{
Serial.begin(115200);
//BT set
Serial1.begin(9600);
Serial.println(" ");
Serial.println("HC-05 started at 9600");
Serial.println("Remember to to set Both NL & CR in the serial monitor.");
Serial.println("Do not enter AT mode");
Serial.println("");
/*
//Speed
pinMode(encoder_pin, INPUT);
attachInterrupt(0, counter, FALLING); //Interrupt 0 is digital pin 2 , Triggers on Falling Edge (change from HIGH to LOW)
pulses = 0;
rpm = 0;
timeold = 0;
*/
//PID
Setpoint = 160; //cm
myPID.SetMode(AUTOMATIC);
myPID.SetSampleTime(100); //ms
myPID.SetControllerDirection(DIRECT);
myPID.SetTunings(0.4, 0, 0); //Kp , Kd, Ki
myPID.SetOutputLimits(-40, 40);
//Sensor
pingTimer[0] = millis() + 75; // First ping start in ms.
for (uint8_t i = 1; i < SONAR_NUM; i++)
pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}
void loop()
{
// listen for communication from the BT module and then write it to the serial monitor
if ( Serial1.available() )
{
c = Serial1.read();
Serial.write( c );
}
// listen for user input and send it to the HC-05
if ( Serial.available() )
{
c = Serial.read();
Serial1.write( c );
}
switch (c)
{
case 'C':
{
for (uint8_t i = 0; i < SONAR_NUM; i++)
{
if (millis() >= pingTimer[i])
{
pingTimer[i] += PING_INTERVAL * SONAR_NUM;
if (i == 0 && currentSensor == SONAR_NUM - 1)
oneSensorCycle(); // Do something with results.
sonar[currentSensor].timer_stop();
currentSensor = i;
cm[currentSensor] = 0;
sonar[currentSensor].ping_timer(echoCheck);
}
}
myPID.Compute();
drive();
debugOutput(); // prints debugging messages to the serial console
//countrpm();
}
break;
case 'G':
{
drive_bt();
}
break;
}
}
void Compute()
{
if (!inAuto) return;
unsigned long now = millis();
int timeChange = (now - lastTime);
if (timeChange >= SampleTime)
{
/*Compute all the working error variables*/
double error = Setpoint - Input;
ITerm += (ki * error);
if (ITerm > outMax) ITerm = outMax;
else if (ITerm < outMin) ITerm = outMin;
double dInput = (Input - lastInput);
/*Compute PID Output*/
Output = kp * error + ITerm - kd * dInput;
if (Output > outMax) Output = outMax;
else if (Output < outMin) Output = outMin;
/*Remember some variables for next time*/
lastInput = Input;
lastTime = now;
}
}
//void countrpm()
//{
// if (millis() - timeold >= 1000)
// {
//Don't process interrupts during calculations
// detachInterrupt(0);
// rpm = (60 * 1000 / pulsesperturn )/ (millis() - timeold)* pulses;
// timeold = millis();
// pulses = 0;
//Restart the interrupt processing
// attachInterrupt(0, counter, FALLING);
// }
// }
void SetTunings(double Kp, double Ki, double Kd)
{
if (Kp < 0 || Ki < 0 || Kd < 0) return;
double SampleTimeInSec = ((double)SampleTime) / 1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;
if (controllerDirection == REVERSE)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
}
void SetSampleTime(int NewSampleTime)
{
if (NewSampleTime > 0)
{
double ratio = (double)NewSampleTime / (double)SampleTime;
ki *= ratio;
kd /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}
void SetOutputLimits(double Min, double Max)
{
if (Min > Max) return;
outMin = Min;
outMax = Max;
if (Output > outMax) Output = outMax;
else if (Output < outMin) Output = outMin;
if (ITerm > outMax) ITerm = outMax;
else if (ITerm < outMin) ITerm = outMin;
}
void SetMode(int Mode)
{
bool newAuto = (Mode == AUTOMATIC);
if (newAuto == !inAuto)
{ /*we just went from manual to auto*/
Initialize();
}
inAuto = newAuto;
}
void Initialize()
{
lastInput = Input;
ITerm = Output;
if (ITerm > outMax) ITerm = outMax;
else if (ITerm < outMin) ITerm = outMin;
}
void SetControllerDirection(int Direction)
{
controllerDirection = Direction;
}
void echoCheck() { // If ping echo, set distance to array.
if (sonar[currentSensor].check_timer())
cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}
void oneSensorCycle()
{ // Do something with the results.
NewTop = sonar_top.ping_cm();
if (NewTop == OldTop)
{
if (Filter1 == OldTop)
{
if (Filter2 == OldTop)
{
if (OldTop > 0)
{
Top = OldTop;
}
}
Filter2 = sonar_top.ping_cm();
}
Filter1 = sonar_top.ping_cm();
}
OldTop = sonar_top.ping_cm();
for (uint8_t i = 0; i < SONAR_NUM; i++)
{
Front = cm[0] * 10;
Input = cm[1] * 10;
}
Serial.println();
}
void drive_bt()
{
motor1.run(FORWARD);
motor4.run(FORWARD);
motor1.setSpeed(Rmotor);
motor4.setSpeed(Lmotor);
switch (c)
{
case 'S':
// stop all motors
{ motor1.run(RELEASE); // stopped
motor4.run(RELEASE); // stopped
c = '*';
}
break;
case 'F':
// turn it on going forward
{ motor1.run(FORWARD);
motor4.run(FORWARD);
c = '*';
}
break;
case 'B':
// turn it on going backward
{ motor1.run(BACKWARD);
motor4.run(BACKWARD);
c = '*';
}
break;
case 'R':
// turn right
{ motor1.run(FORWARD);
motor4.run(BACKWARD);
c = '*';
}
break;
case 'L':
// turn left
{ motor1.run(BACKWARD);
motor4.run(FORWARD);
c = '*';
}
break;
}
}
