FRA221_2015 / Mbed 2 deprecated Project_CAR_B4-B5

class project digital Group B4-B5

Dependencies:   ArduMotoShield ArduinoMotorShield mbed

main.cpp@2:83f5986b5e23, 2015-12-09 (annotated)

Committer:
NorNick
Date:
Wed Dec 09 12:16:19 2015 +0000
Revision:
2:83f5986b5e23
Parent:
0:3aaac6f4553e 
update

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 2:83f5986b5e23 5 AnalogIn current(A2),RV_set(A3),RV2(A4);
NorNick 2:83f5986b5e23 6 DigitalIn mybutton(USER_BUTTON);
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 2:83f5986b5e23 11 float max(float ,float );
NorNick 2:83f5986b5e23 12 float min(float ,float );
NorNick 0:3aaac6f4553e 13
NorNick 0:3aaac6f4553e 14 int main(void){
NorNick 0:3aaac6f4553e 15
NorNick 0:3aaac6f4553e 16 Device.baud(9600);
NorNick 0:3aaac6f4553e 17 pc.baud(9600);
NorNick 0:3aaac6f4553e 18 float value =0;
NorNick 0:3aaac6f4553e 19 float Speed = 0;
NorNick 0:3aaac6f4553e 20 float Left = 0,Right = 0;
NorNick 0:3aaac6f4553e 21 float x=0,y=0;
NorNick 0:3aaac6f4553e 22 char word;
NorNick 0:3aaac6f4553e 23 int i=0,count=0;
NorNick 2:83f5986b5e23 24 float maxAmps = 0,minAmps = 0;
NorNick 2:83f5986b5e23 25
NorNick 0:3aaac6f4553e 26
NorNick 0:3aaac6f4553e 27 pc.printf("Hello PROJECT_CAR.\n");
NorNick 0:3aaac6f4553e 28 while(1)
NorNick 0:3aaac6f4553e 29 {
NorNick 0:3aaac6f4553e 30 // printf("Valuenery = %.2f. %d\n", RV2.read(),abs(i));
NorNick 0:3aaac6f4553e 31 //wait(0.75);
NorNick 2:83f5986b5e23 32 //Motor.forward(1.0,1.0);
NorNick 2:83f5986b5e23 33 //pc.printf(" %.2f\n ",(current.read()*1000+10));
NorNick 2:83f5986b5e23 34 //wait(0.1);
NorNick 2:83f5986b5e23 35 /*float amps = (501 - (current.read()*1000))*27.03/1023;
NorNick 2:83f5986b5e23 36 maxAmps = max(maxAmps , amps);
NorNick 2:83f5986b5e23 37 minAmps = min(minAmps , amps);
NorNick 2:83f5986b5e23 38 float noise = maxAmps - minAmps;
NorNick 2:83f5986b5e23 39 pc.printf("Curent is %.2f A. %.2f ",amps,abs(amps));
NorNick 2:83f5986b5e23 40 pc.printf("Noise is %.2f\n",noise);
NorNick 2:83f5986b5e23 41 if (pc.readable() != -1){maxAmps = amps; minAmps = amps;}
NorNick 2:83f5986b5e23 42 wait(0.1);*/
NorNick 2:83f5986b5e23 43
NorNick 2:83f5986b5e23 44
NorNick 2:83f5986b5e23 45
NorNick 2:83f5986b5e23 46
NorNick 2:83f5986b5e23 47
NorNick 0:3aaac6f4553e 48 if(count<=0)
NorNick 0:3aaac6f4553e 49 {
NorNick 0:3aaac6f4553e 50 if(Device.readable())
NorNick 0:3aaac6f4553e 51 {
NorNick 0:3aaac6f4553e 52 word = Device.getc();
NorNick 0:3aaac6f4553e 53 pc.printf(" %c.\n",word);
NorNick 0:3aaac6f4553e 54 if(word == '%')
NorNick 0:3aaac6f4553e 55 {
NorNick 0:3aaac6f4553e 56 Speed = RV_set.read()*3.3;
NorNick 0:3aaac6f4553e 57 if(Speed >=0 && Speed < 0.825)//0-0.825
NorNick 0:3aaac6f4553e 58 {
NorNick 0:3aaac6f4553e 59 x=0;y=0.824;
NorNick 0:3aaac6f4553e 60 pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=0,x,y,RV_set.read(),Speed);
NorNick 0:3aaac6f4553e 61 }
NorNick 0:3aaac6f4553e 62 else if(Speed >= 0.825 && Speed < 1.650)//0.825-1.650
NorNick 0:3aaac6f4553e 63 {
NorNick 0:3aaac6f4553e 64 x=0.825;y=1.649;
NorNick 0:3aaac6f4553e 65 pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=1,x,y,RV_set.read(),Speed);
NorNick 0:3aaac6f4553e 66 }
NorNick 0:3aaac6f4553e 67 else if(Speed >= 1.650 && Speed < 2.475)//1.650-2.475
NorNick 0:3aaac6f4553e 68 {
NorNick 0:3aaac6f4553e 69 x=1.650;y=2.474;
NorNick 0:3aaac6f4553e 70 pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=2,x,y,RV_set.read(),Speed);
NorNick 0:3aaac6f4553e 71 }
NorNick 0:3aaac6f4553e 72 else//2.475-3.300
NorNick 0:3aaac6f4553e 73 {
NorNick 0:3aaac6f4553e 74 x=2.475;y=3.300;
NorNick 0:3aaac6f4553e 75 pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=3,x,y,RV_set.read(),Speed);
NorNick 0:3aaac6f4553e 76 }
NorNick 0:3aaac6f4553e 77 count++;
NorNick 0:3aaac6f4553e 78 }
NorNick 0:3aaac6f4553e 79 if(word == '#')
NorNick 0:3aaac6f4553e 80 {
NorNick 2:83f5986b5e23 81 //Device.printf("24.3,324.5,234.4,\n");
NorNick 0:3aaac6f4553e 82 Device.printf("%.2f,",0);//ความเร็ว
NorNick 2:83f5986b5e23 83 Device.printf("%.2f,",0);//กระแส
NorNick 0:3aaac6f4553e 84 if(RV_set.read()>=0&&RV_set.read()<0.25)
NorNick 2:83f5986b5e23 85 Device.printf("Kange,\n");//โหมด
NorNick 0:3aaac6f4553e 86 else if(RV_set.read()>=0.25&&RV_set.read()<0.50)
NorNick 2:83f5986b5e23 87 Device.printf("Thin,\n");//โหมด
NorNick 0:3aaac6f4553e 88 else if(RV_set.read()>=0.50&&RV_set.read()<0.75)
NorNick 2:83f5986b5e23 89 Device.printf("Long,\n");//โหมด
NorNick 0:3aaac6f4553e 90 else
NorNick 2:83f5986b5e23 91 Device.printf("FAT,\n");//โหมด
NorNick 2:83f5986b5e23 92
NorNick 0:3aaac6f4553e 93 }
NorNick 0:3aaac6f4553e 94 }
NorNick 0:3aaac6f4553e 95 wait(0.5);
NorNick 0:3aaac6f4553e 96 }
NorNick 0:3aaac6f4553e 97 else
NorNick 0:3aaac6f4553e 98 {
NorNick 0:3aaac6f4553e 99 if(Device.readable())
NorNick 0:3aaac6f4553e 100 {
NorNick 0:3aaac6f4553e 101 if(RV_set.read()>=0.03)
NorNick 0:3aaac6f4553e 102 {
NorNick 0:3aaac6f4553e 103 if(RV2.read()<0.49)
NorNick 0:3aaac6f4553e 104 {
NorNick 0:3aaac6f4553e 105 Right = y/3.3;
NorNick 0:3aaac6f4553e 106 Left = RV2.read()/2+(0.25*i);
NorNick 0:3aaac6f4553e 107 pc.printf("Value = %.2f , %.2f , %.2f.\n",RV2.read(),Left,Right);
NorNick 0:3aaac6f4553e 108 }
NorNick 0:3aaac6f4553e 109 else if(RV2.read()>=0.49&&RV2.read()<=0.51)
NorNick 0:3aaac6f4553e 110 {
NorNick 2:83f5986b5e23 111 Left = (y/3.3)-0.072;
NorNick 0:3aaac6f4553e 112 Right = y/3.3;
NorNick 0:3aaac6f4553e 113 pc.printf("CentF = %.2f , %.2f , %.2f\n",Left,Right,RV2.read());
NorNick 0:3aaac6f4553e 114 }
NorNick 0:3aaac6f4553e 115 else
NorNick 0:3aaac6f4553e 116 {
NorNick 0:3aaac6f4553e 117 Left = y/3.3;
NorNick 0:3aaac6f4553e 118 Right = (((RV2.read()*2)-2)*(-1)/4)+(0.25*i);
NorNick 0:3aaac6f4553e 119 pc.printf("Right = %.2f , %.2f , %.2f.\n",RV2.read(),Left,Right);
NorNick 0:3aaac6f4553e 120 }
NorNick 0:3aaac6f4553e 121 Motor.forward(Left,Right);
NorNick 2:83f5986b5e23 122 float amps = (501 - (current.read()*1000))*27.03/1023;
NorNick 2:83f5986b5e23 123 maxAmps = max(maxAmps , amps);
NorNick 2:83f5986b5e23 124 minAmps = min(minAmps , amps);
NorNick 2:83f5986b5e23 125 float noise = maxAmps - minAmps;
NorNick 2:83f5986b5e23 126 Device.printf("%.2f,",abs(amps));//กระแส
NorNick 0:3aaac6f4553e 127 Device.printf("%.2f,",((Left+Right)/2));//ความเร็ว
NorNick 2:83f5986b5e23 128 pc.printf("Current is %.2f\n",abs(amps));
NorNick 0:3aaac6f4553e 129 if(1==0)
NorNick 2:83f5986b5e23 130 Device.printf("Khang ,\n");//โหมด
NorNick 0:3aaac6f4553e 131 else if(i==1)
NorNick 2:83f5986b5e23 132 Device.printf("Thin,\n");//โหมด
NorNick 0:3aaac6f4553e 133 else if(i==2)
NorNick 2:83f5986b5e23 134 Device.printf("Long,\n");//โหมด
NorNick 0:3aaac6f4553e 135 else
NorNick 2:83f5986b5e23 136 Device.printf("FAT,\n");//โหมด
NorNick 2:83f5986b5e23 137 if (pc.readable() != -1){maxAmps = amps; minAmps = amps;}
NorNick 2:83f5986b5e23 138 wait(0.8);
NorNick 0:3aaac6f4553e 139 }
NorNick 0:3aaac6f4553e 140 }
NorNick 0:3aaac6f4553e 141 }
NorNick 0:3aaac6f4553e 142 }
NorNick 0:3aaac6f4553e 143 //Motor.stop();
NorNick 0:3aaac6f4553e 144 }
NorNick 2:83f5986b5e23 145 float max(float a,float b)
NorNick 0:3aaac6f4553e 146 {
NorNick 2:83f5986b5e23 147 if(a>b)
NorNick 2:83f5986b5e23 148 return a;
NorNick 2:83f5986b5e23 149 else
NorNick 2:83f5986b5e23 150 return b;
NorNick 2:83f5986b5e23 151 }
NorNick 2:83f5986b5e23 152 float min(float a,float b)
NorNick 2:83f5986b5e23 153 {
NorNick 2:83f5986b5e23 154 if(a<b)
NorNick 2:83f5986b5e23 155 return a;
NorNick 2:83f5986b5e23 156 else
NorNick 2:83f5986b5e23 157 return b;
NorNick 0:3aaac6f4553e 158 }