FRA221_2015 / Mbed 2 deprecated Project_CAR_B4-B5

class project digital Group B4-B5

Dependencies:   ArduMotoShield ArduinoMotorShield mbed

main.cpp@0:3aaac6f4553e, 2015-12-09 (annotated)

Committer:
NorNick
Date:
Wed Dec 09 04:08:35 2015 +0000
Revision:
0:3aaac6f4553e
Child:
2:83f5986b5e23
For Final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NorNick 0:3aaac6f4553e 1 #include "ArduMotoShield.h"
NorNick 0:3aaac6f4553e 2 #include "mbed.h"
NorNick 0:3aaac6f4553e 3
NorNick 0:3aaac6f4553e 4 ArduMotoShield Motor;
NorNick 0:3aaac6f4553e 5 AnalogIn EMS(A0),A(A1),current(A2),RV_set(A3),RV2(A4);
NorNick 0:3aaac6f4553e 6 DigitalIn mybutton(USER_BUTTON),L339(D4);
NorNick 0:3aaac6f4553e 7
NorNick 0:3aaac6f4553e 8 Serial Device(D8, D2);
NorNick 0:3aaac6f4553e 9 Serial pc(SERIAL_TX, SERIAL_RX);
NorNick 0:3aaac6f4553e 10
NorNick 0:3aaac6f4553e 11 void swap(float &x,float &y);
NorNick 0:3aaac6f4553e 12
NorNick 0:3aaac6f4553e 13 int main(void){
NorNick 0:3aaac6f4553e 14
NorNick 0:3aaac6f4553e 15 Device.baud(9600);
NorNick 0:3aaac6f4553e 16 pc.baud(9600);
NorNick 0:3aaac6f4553e 17 float value =0;
NorNick 0:3aaac6f4553e 18 float Speed = 0;
NorNick 0:3aaac6f4553e 19 float Left = 0,Right = 0;
NorNick 0:3aaac6f4553e 20 float x=0,y=0;
NorNick 0:3aaac6f4553e 21 char word;
NorNick 0:3aaac6f4553e 22 int i=0,count=0;
NorNick 0:3aaac6f4553e 23
NorNick 0:3aaac6f4553e 24 pc.printf("Hello PROJECT_CAR.\n");
NorNick 0:3aaac6f4553e 25 while(1)
NorNick 0:3aaac6f4553e 26 {
NorNick 0:3aaac6f4553e 27 // printf("Valuenery = %.2f. %d\n", RV2.read(),abs(i));
NorNick 0:3aaac6f4553e 28 //wait(0.75);
NorNick 0:3aaac6f4553e 29 Motor.stop();
NorNick 0:3aaac6f4553e 30 if(count<=0)
NorNick 0:3aaac6f4553e 31 {
NorNick 0:3aaac6f4553e 32 if(Device.readable())
NorNick 0:3aaac6f4553e 33 {
NorNick 0:3aaac6f4553e 34 word = Device.getc();
NorNick 0:3aaac6f4553e 35 pc.printf(" %c.\n",word);
NorNick 0:3aaac6f4553e 36 if(word == '%')
NorNick 0:3aaac6f4553e 37 {
NorNick 0:3aaac6f4553e 38 Speed = RV_set.read()*3.3;
NorNick 0:3aaac6f4553e 39 if(Speed >=0 && Speed < 0.825)//0-0.825
NorNick 0:3aaac6f4553e 40 {
NorNick 0:3aaac6f4553e 41 x=0;y=0.824;
NorNick 0:3aaac6f4553e 42 pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=0,x,y,RV_set.read(),Speed);
NorNick 0:3aaac6f4553e 43 }
NorNick 0:3aaac6f4553e 44 else if(Speed >= 0.825 && Speed < 1.650)//0.825-1.650
NorNick 0:3aaac6f4553e 45 {
NorNick 0:3aaac6f4553e 46 x=0.825;y=1.649;
NorNick 0:3aaac6f4553e 47 pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=1,x,y,RV_set.read(),Speed);
NorNick 0:3aaac6f4553e 48 }
NorNick 0:3aaac6f4553e 49 else if(Speed >= 1.650 && Speed < 2.475)//1.650-2.475
NorNick 0:3aaac6f4553e 50 {
NorNick 0:3aaac6f4553e 51 x=1.650;y=2.474;
NorNick 0:3aaac6f4553e 52 pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=2,x,y,RV_set.read(),Speed);
NorNick 0:3aaac6f4553e 53 }
NorNick 0:3aaac6f4553e 54 else//2.475-3.300
NorNick 0:3aaac6f4553e 55 {
NorNick 0:3aaac6f4553e 56 x=2.475;y=3.300;
NorNick 0:3aaac6f4553e 57 pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=3,x,y,RV_set.read(),Speed);
NorNick 0:3aaac6f4553e 58 }
NorNick 0:3aaac6f4553e 59 count++;
NorNick 0:3aaac6f4553e 60 }
NorNick 0:3aaac6f4553e 61 if(word == '#')
NorNick 0:3aaac6f4553e 62 {
NorNick 0:3aaac6f4553e 63 value = current.read()*1023;
NorNick 0:3aaac6f4553e 64 Device.printf("%.2f,",0);//ความเร็ว
NorNick 0:3aaac6f4553e 65 Device.printf("%.2f,",(((value-490)*5/1023)/0.0645));//กระแส
NorNick 0:3aaac6f4553e 66 if(RV_set.read()>=0&&RV_set.read()<0.25)
NorNick 0:3aaac6f4553e 67 Device.printf(" แห้ง ,\n");//โหมด
NorNick 0:3aaac6f4553e 68 else if(RV_set.read()>=0.25&&RV_set.read()<0.50)
NorNick 0:3aaac6f4553e 69 Device.printf(" ผอม ,\n");//โหมด
NorNick 0:3aaac6f4553e 70 else if(RV_set.read()>=0.50&&RV_set.read()<0.75)
NorNick 0:3aaac6f4553e 71 Device.printf(" อวบ ,\n");//โหมด
NorNick 0:3aaac6f4553e 72 else
NorNick 0:3aaac6f4553e 73 Device.printf(" อ้วน ,\n");//โหมด
NorNick 0:3aaac6f4553e 74 }
NorNick 0:3aaac6f4553e 75 }
NorNick 0:3aaac6f4553e 76 wait(0.5);
NorNick 0:3aaac6f4553e 77 }
NorNick 0:3aaac6f4553e 78 else
NorNick 0:3aaac6f4553e 79 {
NorNick 0:3aaac6f4553e 80 if(Device.readable())
NorNick 0:3aaac6f4553e 81 {
NorNick 0:3aaac6f4553e 82 if(RV_set.read()>=0.03)
NorNick 0:3aaac6f4553e 83 {
NorNick 0:3aaac6f4553e 84 if(RV2.read()<0.49)
NorNick 0:3aaac6f4553e 85 {
NorNick 0:3aaac6f4553e 86 Right = y/3.3;
NorNick 0:3aaac6f4553e 87 Left = RV2.read()/2+(0.25*i);
NorNick 0:3aaac6f4553e 88 pc.printf("Value = %.2f , %.2f , %.2f.\n",RV2.read(),Left,Right);
NorNick 0:3aaac6f4553e 89 }
NorNick 0:3aaac6f4553e 90 else if(RV2.read()>=0.49&&RV2.read()<=0.51)
NorNick 0:3aaac6f4553e 91 {
NorNick 0:3aaac6f4553e 92 Left = y/3.3;
NorNick 0:3aaac6f4553e 93 Right = y/3.3;
NorNick 0:3aaac6f4553e 94 pc.printf("CentF = %.2f , %.2f , %.2f\n",Left,Right,RV2.read());
NorNick 0:3aaac6f4553e 95 }
NorNick 0:3aaac6f4553e 96 else
NorNick 0:3aaac6f4553e 97 {
NorNick 0:3aaac6f4553e 98 Left = y/3.3;
NorNick 0:3aaac6f4553e 99 Right = (((RV2.read()*2)-2)*(-1)/4)+(0.25*i);
NorNick 0:3aaac6f4553e 100 pc.printf("Right = %.2f , %.2f , %.2f.\n",RV2.read(),Left,Right);
NorNick 0:3aaac6f4553e 101 }
NorNick 0:3aaac6f4553e 102 Motor.forward(Left,Right);
NorNick 0:3aaac6f4553e 103 value = current.read()*1023;
NorNick 0:3aaac6f4553e 104 Device.printf("%.2f,",((Left+Right)/2));//ความเร็ว
NorNick 0:3aaac6f4553e 105 Device.printf("%.2f,",(((value-490)*5/1023)/0.0645));//กระแส
NorNick 0:3aaac6f4553e 106 if(1==0)
NorNick 0:3aaac6f4553e 107 Device.printf(" แห้ง ,\n");//โหมด
NorNick 0:3aaac6f4553e 108 else if(i==1)
NorNick 0:3aaac6f4553e 109 Device.printf(" ผอม ,\n");//โหมด
NorNick 0:3aaac6f4553e 110 else if(i==2)
NorNick 0:3aaac6f4553e 111 Device.printf(" อวบ ,\n");//โหมด
NorNick 0:3aaac6f4553e 112 else
NorNick 0:3aaac6f4553e 113 Device.printf(" อ้วน ,\n");//โหมด
NorNick 0:3aaac6f4553e 114 wait(0.5);
NorNick 0:3aaac6f4553e 115 }
NorNick 0:3aaac6f4553e 116 }
NorNick 0:3aaac6f4553e 117 }
NorNick 0:3aaac6f4553e 118 }
NorNick 0:3aaac6f4553e 119 //Motor.stop();
NorNick 0:3aaac6f4553e 120 }
NorNick 0:3aaac6f4553e 121
NorNick 0:3aaac6f4553e 122 void swap(float &x,float &y)
NorNick 0:3aaac6f4553e 123 {
NorNick 0:3aaac6f4553e 124 float temp;
NorNick 0:3aaac6f4553e 125 temp = x;
NorNick 0:3aaac6f4553e 126 x = y;
NorNick 0:3aaac6f4553e 127 y = temp;
NorNick 0:3aaac6f4553e 128 }