Abhinaba Bhattacharjee
For all
Dependencies: LidarLitev2 Servo mbed
AutoRoboMotion.cpp
- Committer:
- AbhiBjee
- Date:
- 2017-05-05
- Revision:
- 0:32adce4ab239
File content as of revision 0:32adce4ab239:
#include "mbed.h"
//#include "Map.hpp"
#include "LidarLitev2.h"
#include "Servo.h"// The desired Servo sweep range is 60 degrees
// But due to some mechanical Load errors it has been calibrated to 0 to 70 degrees
// The front facing position varies from 27 degress to 38 degrees due to the servo motion slip.
// Variable declaration
Servo myservo(D3);// Object to declare the servo functionalities
float pos = 33;// Default avg front facing posiion of the Servo
Servo LidarServo(D7);
int L_Dist;
float LSrvoPos;
DigitalIn IR1(A0);
DigitalIn IR2(A1);
int L2Cnt=0;
int L1Cnt=0;
int LstVal1 =0;
int LstVal2 =0;
DigitalIn OA(D4);
DigitalIn OA2(D2);
int Tsop1_Val;
int i;
int j;
int i1;
int j1;
float Dist_Val;
int LsrvMin = 10;
int LsrvMax =90;
float Val;
Timer dt;
int Cnt=0;
int Flg2=0; // For Sensor activation
int Flg=0; // Flag to enable motions (Forward, Backward, Stop state)
int Flg2St = 0;// Unused Variable
float Level = 0.30;// Variable to store the PWM Levels activated by the gear Button in the App.
float pwm_Level;// Unused Variable
// Function Initialisation
void handleInput(int in);
void drive_forward(float level);
void drive_backward(float level);
void Robot_Stop();
float LidarSense(int minpos, int maxpos);
LidarLitev2 Lidar(PTC11, PTC10);// Tx Rx for I2C Communication in Lidar
Serial pc(USBTX,USBRX);//Tx Rx for Serial Communication (UART)(PC to Processor)
Serial BT(PTC15,PTC14);// Tx Rx for Bluetooth Communication ()
PwmOut ENApwm(D5); // Motor driver Enable A
PwmOut ENBpwm(D6); // Motor driver Enable B
// Setting coresponding pins to output mode for Motor movements
DigitalOut LeftMotor_P1(D8);
DigitalOut LeftMotor_P2(D9);
DigitalOut RightMotor_P1(D11);
DigitalOut RightMotor_P2(D10);
// The main Function
int main() {
pc.baud(9600);// UART communication in 9600 bits per seconds
BT.baud(9600);// Bluetooth communication in 9600 bits per seconds
// Printing some execution statements to ensure the controller is communicating properly.
pc.printf(" Hello User!\r\n");
printf(" Are You Ready \r\n");
// At the start set servo to its default front facing position
if(pos>33){
myservo = (27/100.0);
}
else{
myservo = (35/100.0);
}
Lidar.configure();
dt.start();
while(1){ // run an infinite loop to read blutooth data & call corresponding navigation function
if (BT.readable()){ // Read incoming Bluetooth Data
int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data
//int i = atoi((int)BT_Val); // Convert it to Interger.
//BT_Val -='0';
pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
// Bluetooth data below decimal 25 contains certain values for the button operations
// Bluetooth data above 25 triggers the PWM pins of EnA and EnB for motor speed control
if (BT_Val >= 30 && BT_Val <=40){
float Val = (((float)BT_Val - 30.0)*(100.0 - 0.0)/(40.0 - 30.0) + 0.0 );
Level = ((float)Val/100);
pc.printf("%f \r\n",Level);
//float pwm_Level = Level;
}
else if (BT_Val < 25){
handleInput(BT_Val);
}
else if (BT_Val > 41){// accelerometer of phone for motion control
float Newpos = (((float)BT_Val - 70.0)*(70.0 - 0.0)/(210.0 - 70.0) + 0.0 );
myservo = (Newpos/100.0);
/*BT_Val_diff = BT_Val-LstBT_Val;
pos = pos+ (BT_Val_diff*0.5);
myservo = (pos/100.0);*/
}
while (Flg2 ==1){
Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax);
if (BT.readable()){ // Read incoming Bluetooth Data
int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data
pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
if (BT_Val < 25){
handleInput(BT_Val);
}
}
if (Flg == 1){
drive_forward((float)Level);
}
else if (Flg == 2){
if(Dist_Val < 150){
Val=50.0f;
Level = ((float)Val/100);
}
drive_backward((float)Level);
}
else if (Flg == 0){
Robot_Stop();
}
// int L_Dist=Lidar.distance();
// dt.reset();
// LidarServo = (45/100.0);
if (Dist_Val< 150.0){
LsrvMin = 10;
LsrvMax = 90;
Val=0.0;
Level = ((float)Val/100);
Cnt = Cnt+1;
}
else if ((Dist_Val> 150.0)&&(Dist_Val<= 300.0)){
//LsrvMin=
LsrvMin = (((float)Dist_Val - 150.0)*(40.0 - 10.0)/(300.0 - 150.0) + 10.0 );
LsrvMax = (((float)Dist_Val - 150.0)*(60.0 - 90.0)/(300.0 - 150.0) + 90.0 );
Val = (((float)Dist_Val - 150.0)*(50.0 - 0.0)/(300.0 - 150.0) + 0.0 );
Level = ((float)Val/100);
}
else if (Dist_Val> 300.0){
LsrvMin = 40;
LsrvMax = 70;
Val=100.0;
Level = ((float)Val/100);
}
if (Cnt == 10){
Flg2= 2;
Cnt=0;
break;
}
int Tsop2_Val = OA2.read();
int Tsop1_Val = OA.read();
if ((Flg==2)&& (Tsop1_Val==1)){
drive_backward((float)Level);
}
else if ((Flg==2)&& (Tsop1_Val==0)){
Robot_Stop();
Flg=0;
}
if ((Flg==1)&& (Tsop2_Val==1)){
drive_forward((float)Level);
}
else if ((Flg==1)&& (Tsop2_Val==0)){
Robot_Stop();
Flg=0;
}
continue;
// break;
}
/* if (Flg2 == Flg2St) && ()
if (Flg2 ==1){
Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax);
}
else if (Flg2 ==0) {
Robot_Stop();
}
}
else{
if (Flg2 ==1){
Dist_Val = LidarSense((int)LsrvMin,(int)LsrvMax);
}
else if (Flg2 ==0) {
Robot_Stop();
}
}*/
//pc.printf("Flg2 = %d", Flg2);
while(Flg2 ==2){
int Tsop1_Val = OA.read();
if(Tsop1_Val==1){
Val=40.0;
Level = ((float)Val/100);
drive_backward((float)Level);
if(Cnt == 5){
Cnt=0;
Robot_Stop();
}
//wait(2);
}
else if (Tsop1_Val == 0){
Robot_Stop();
Flg=0;
break;
}
Cnt=Cnt+1;
}
/*while(Flg2 == 4){
if (BT.readable()){ // Read incoming Bluetooth Data
int BT_Val = (int)(pc.putc(BT.getc()));// get the Ascii value of the incomming 1 byte data
pc.printf("%d \r\n",BT_Val); // print the Bluetooth data in serial Monitor for cross validation
if (BT_Val < 25){
handleInput(BT_Val);
}
}
Val=30.0;
Level = ((float)Val/100);
LidarServo = (45/100.0);
L_Dist=Lidar.distance();
LSrvoPos = LidarServo.read();
dt.reset();
int value1 = IR1.read();
int value2 = IR2.read();
if ((value1==1)&&(value1>=LstVal1)){
L1Cnt=L1Cnt+1;
}
else if(value1<LstVal1){
L1Cnt=0;
}
if ((value2==1)&&(value2>=LstVal2)){
L2Cnt=L2Cnt+1;
}
else if(value2<LstVal2){
L2Cnt=0;
}
//int ServoPos = (int)((myservo.read())*100);
int pos = ((int)((myservo.read())*100.0f));
if ((L1Cnt>5)&&(L2Cnt<5)){
pos = pos-3;
myservo = (pos/100.0);
//wait(0.01);
pos = pos+1;
myservo = (pos/100.0);
if (pos < 24){
pos = 24;
}
}
else if ((L2Cnt>5)&&(L1Cnt<5)){
pos = pos+3;
myservo = (pos/100.0);
//wait(0.01);
pos = pos-1;
myservo = (pos/100.0);
if (pos > 38){
pos = 38;
}
}
else if ((L2Cnt>=5)&&(L1Cnt>=5)){
continue;
}
if (Flg == 1){
drive_forward((float)Level);
}
else if (Flg == 2){
drive_backward((float)Level);
}
else if (Flg == 0){
Robot_Stop();
break;
}
int Tsop2_Val = OA2.read();
int Tsop1_Val = OA.read();
if ((Flg==2)&& (Tsop1_Val==1)){
drive_backward((float)Level);
}
else if ((Flg==2)&& (Tsop1_Val==0)){
Robot_Stop();
Flg=0;
break;
}
/*if ((Flg==1)&& (Tsop2_Val==1)){
drive_forward((float)Level);
}
else if ((Flg==1)&& (Tsop2_Val==0)){
Robot_Stop();
Flg=0;
break;
}
if (L_Dist<50){
Robot_Stop();
Flg=0;
break;
}
LstVal1=value1;
LstVal2=value2;
continue;
}*/
if (Flg == 1){
drive_forward((float)Level);
}
else if (Flg == 2){
drive_backward((float)Level);
}
else if (Flg == 0){
Robot_Stop();
}
// while (Flg == 2)
//if BT_Val == 'B'
}
//LstBT_Val=BT_Val;
//Flg2St = Flg2;
int Tsop2_Val = OA2.read();
int Tsop1_Val = OA.read();
if ((Flg==2)&& (Tsop1_Val==1)){
drive_backward(0.3f);
}
else if ((Flg==2)&& (Tsop1_Val==0)){
Robot_Stop();
Flg=0;
}
if ((Flg==1)&& (Tsop2_Val==1)){
drive_forward((float)Level);
}
else if ((Flg==1)&& (Tsop2_Val==0)){
Robot_Stop();
Flg=0;
}
}
}
// Function which calls specific execution for corresponding incomming Serial inputs from Bluetooth
void handleInput(int in) {
switch(in) {
//Motor Rotation Controls
case 6 :// Forward Motion
Flg = 1;
break;
case 7 :
Flg = 2;// Backward Motion
break;
case 0 : // Robot Stop
Flg = 0;
Flg2 = 0;
break;
case 13 : // Robot Stop
Flg2 = 1;
break;
case 14 : // Robot Stop
int pos = (int)((myservo.read())*100);
if(pos>33){
myservo = (27/100.0);
}
else{
myservo = (35/100.0);
}
Flg2 = 4;
break;
//Servo Rotation Controls
case 1:// Turn 30 degree Left and back to front facing
if (pos >0){
myservo = (pos/100.0);
for( j1=pos; j1>=0; j1--) {
myservo = (j1/100.0);
//printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=j1;
for(i1=pos; i1<=35; i1++) {
myservo = (i1/100.0);
//printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=i1;
}
else{
myservo = (0/100.0);
pos = 0;
for(i1=pos; i1<=35; i1++) {
myservo = (i1/100.0);
//printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=i1;
}
break;
case 2: //Turn 30 degree Left
if (pos >0){
//myservo = (pos/100.0);
for( j1=pos; j1>=0; j1--) {
myservo = (j1/100.0);
// printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=j1;
}
else{
myservo = (0/100.0);
}
break;
case 3: // Sweep Servo to forward position irrespective of its current position
if(pos >40){
//myservo = (pos/100.0);
for( j=pos; j>=27; j--) {
myservo = (j/100.0);
//printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=j;
}
else if (pos < 25){
//myservo = (pos/100.0);
for(i1=pos; i1<=35; i1++) {
myservo = (i1/100.0);
//printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=i1;
}
else{
myservo = (pos/100.0);
}
break;
case 4://Turn 30 degree Right
if (pos <61){
// myservo = (pos/100.0);
for(i=pos; i<=60; i++) {
myservo = (i/100.0);
//printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=i;
}
else{
myservo = (60/100.0);
}
break;
case 5:// Turn 30 degree Right and back to front facing
if (pos <61){
//myservo = (pos/100.0);
for(i=pos; i<=60; i++) {
myservo = (i/100.0);
//printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=i;
for( j=pos; j>=27; j--) {
myservo = (j/100.0);
//printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=j;
}
else{
myservo = (60/100.0);
pos = 60;
for( j=pos; j>=27; j--) {
myservo = (j/100.0);
//printf("%f\r\n",myservo.read());
wait(0.01);
}
pos=j;
}
//break;
break;
case 8:// Turn Servo Left by 5 degrees irrespective of its current position
pos = pos-5;
myservo = (pos/100.0);
if (pos < 0){
pos = 0;
}
break;
case 9: // Turn Servo right by 5 degrees irrespective of its current position
pos = pos+5;
myservo = (pos/100.0);
if (pos > 60){
pos = 60;
}
break;
default:
pc.printf("I don't know: \"%d\"\n", in);
break;
}
}
// Function to move rear wheel DC motors forward
void drive_forward(float level){
ENApwm.write(level);
ENBpwm.write(level);
LeftMotor_P1=1;
LeftMotor_P2=0;
RightMotor_P1=1;
RightMotor_P2=0;
}
// Function to move rear wheel DC motors backward
void drive_backward(float level){
ENApwm.write(level);
ENBpwm.write(level);
LeftMotor_P1=0;
LeftMotor_P2=1;
RightMotor_P1=0;
RightMotor_P2=1;
}
// Function to stop rear wheel motion
void Robot_Stop() {
//ENApwm.write(level);
//ENBpwm.write(level);
LeftMotor_P1=1;
LeftMotor_P2=1;
RightMotor_P1=1;
RightMotor_P2=1;
}
float LidarSense(int minpos, int maxpos){
float sum=0.0;
int cnt =0;
float Avg = 0.0;
// if (Flg2 == 1) {
for(int i=minpos; i<=maxpos; i++) {
LidarServo = (i/100.0);
L_Dist=Lidar.distance();
LSrvoPos = LidarServo.read();
//pc.printf("D %d P %f\n\r",L_Dist ,LSrvoPos);
dt.reset();
sum = sum+L_Dist;
cnt = cnt+1;
/*if (Flg == 1){
drive_forward((float)Level);
}
else if (Flg == 2){
drive_backward((float)Level);
}
else if (Flg == 0){
Robot_Stop();
} */
//printf("%f\r\n",myservo.read());
wait(0.008);
}
for(int i=maxpos; i>=minpos; i--) {
LidarServo = (i/100.0);
L_Dist=Lidar.distance();
LSrvoPos = LidarServo.read();
//pc.printf("D %d P %f\n\r",L_Dist,LSrvoPos);
//pc.printf("distance = %d cm frequency = %.2f Hz ServoPos = %f degrees\n\r", Lidar.distance(), 1/dt.read(),LidarServo.read());
dt.reset();
sum = sum+L_Dist;
cnt = cnt+1;
//printf("%f\r\n",myservo.read());
/*if (Flg == 1){
drive_forward((float)Level);
}
else if (Flg == 2){
drive_backward((float)Level);
}
else if (Flg == 0){
Robot_Stop();
} */
wait(0.008);
}
Avg = sum/cnt;
pc.printf("flg2 = %d \r\n",Flg2);
pc.printf("Avg = %f \r\n",Avg);
return Avg;
//LidarSense((int)LsrvMin,(int)LsrvMax);
// }
//else{
// Flg2=0;
// }
}
/* int LidarRotate(int pos){
int Max_L_Dist =0;
int Max_LsrvPos = 0;
for(int i= pos; i<=99; i++) {
LidarServo = (i/100.0);
L_Dist=Lidar.distance();
LSrvoPos = LidarServo.read();
if (L_Dist > Max_L_Dist) {
Max_L_Dist = L_Dist;
Max_LsrvPos= i;
}
// cnt = cnt+1;
//}
wait(0.008);
lst_L_Dist = L_Dist;
}*/
// }