For all
Dependencies: LidarLitev2 Servo mbed
Revision 0:32adce4ab239, committed 2017-05-05
- Comitter:
- AbhiBjee
- Date:
- Fri May 05 19:01:21 2017 +0000
- Commit message:
- FRDM Smart Car code by Abhinaba
Changed in this revision
diff -r 000000000000 -r 32adce4ab239 AutoRoboMotion.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutoRoboMotion.cpp Fri May 05 19:01:21 2017 +0000
@@ -0,0 +1,639 @@
+#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;
+ }*/
+
+
+
+ // }
+
\ No newline at end of file
diff -r 000000000000 -r 32adce4ab239 LidarLitev2.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LidarLitev2.lib Fri May 05 19:01:21 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/sventura3/code/LidarLitev2/#7e6c07be1754
diff -r 000000000000 -r 32adce4ab239 Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri May 05 19:01:21 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r 32adce4ab239 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri May 05 19:01:21 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/97feb9bacc10 \ No newline at end of file