Embedded_Camp
course1
Dependencies: WS2812 PixelArray Adafruit_GFX HC_SR04_Ultrasonic_Library
Diff: main.cpp
- Revision:
- 101:18e7ac07b620
- Parent:
- 99:43e306bf6f13
--- a/main.cpp Mon Jun 17 02:36:43 2019 +0000
+++ b/main.cpp Sun Aug 04 12:24:12 2019 +0000
@@ -31,32 +31,12 @@
Timer timer;
PixelArray px(WS2812_BUF);
WS2812 ws(D7, WS2812_BUF, 7, 15, 10, 15);
-// Thread thread1;
-// Thread thread2;
-unsigned int sensorValues[button_SENSORS];
-
-// Error variables
-static int prev_err = 0;
-static int int_err = 0;
-int err = 0;
+volatile int button = 0;
-// PID & velocity value
-static float pval = 0.15;
-static float ival = 0.00003;
-static float dval = 2.3;
-static float vel = 120.0;
-
-volatile int button = 0;
-int position = 0;
-int dist = 0;
-int cnt = 0;
-float t = 0;
-
-// Reflective sensor
-bool rightObs= false;
-bool leftObs = false;
+// PID variables
+static float pval = 0;
// motor Driver
float fPwmAPeriod;
@@ -64,88 +44,34 @@
float fPwmBPeriod;
float fPwmBPulsewidth;
-// opearte ultrasonic
-//void ultrasonic_thread()
-//{
-// while (1)
-// {
-// ultra.setMode(false); // have updates every .1 seconds and try only once.
-// ultra.trig();
-// dist = ultra.getDistance();
-// pc.printf("Detected value: %d\r\n", dist);
-// }
-//}
-
-
-// opearte reflective sensors
-/*
-void reflective_thread()
-{
- while(1){
- char data_write[2];
- char data_read[2];
- i2c.frequency(100000);
- i2c.read((PCF8574_ADDR<<1|0x01), data_read, 2, 0);
- pc.printf("hexa: %x, %x\r\n", data_read[0], data_read[1]);
- if(data_read[0]>>6 == 0b10)
- {
- pc.printf("Obstacle on left!\r\n");
- data_write[0] = 0xDF;
- leftObs = true;
- }
- else if(data_read[0]>>6 == 0b01)
- {
- pc.printf("Obstacle on right!\r\n");
- data_write[0] = 0xDF;
- rightObs = true;
- }
- else if(data_read[0]>>6 == 0b00)
- {
- pc.printf("Obstacle on front!\r\n");
- data_write[0] = 0xDF;
- leftObs = true;
- rightObs = true;
- wait(3);
- }
-
- else
- {
- data_write[0]=0xFF;
- }
- i2c.write((PCF8574_ADDR<<1),data_write,1,0);
- }
-}*/
-
inline void update_display(){
gOled2.clearDisplay();
gOled2.setTextCursor(0,0);
gOled2.printf("E-RON alphabot\r\n");
- gOled2.printf("Timer: %.2f\r\n",t);
- gOled2.printf("V: %.0f\r\n",vel);
gOled2.printf("P: %.2f\r\n",pval);
- gOled2.printf("I: %.5f\r\n",ival);
- gOled2.printf("D: %.2f\r\n",dval);
gOled2.display();
}
- int colorbuf[NUM_COLORS] = {0xff0000,0x00ff00,0x0000ff};
- int colorbuf3 =0x000000;
+
+int colorbuf[NUM_COLORS] = {0xff0000,0x00ff00,0x0000ff};
+int colorbuf3 =0x000000;
int main()
-{
+{
+
for (int i = 0; i < WS2812_BUF; i++) {
px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
}
+
for (int j=0; j<WS2812_BUF; j++) {
// px.SetI(pixel position, II value)
px.SetI(j%WS2812_BUF, 0xf+(0xf*4));
}
- // thread1.start(ultrasonic_thread);
- // thread2.start(reflective_thread);
+ update_display();
+
RemoteIR::Format format;
uint8_t buf[32];
int bitcount;
- update_display();
while(1)
{ // read the value of the code
if (IR.getState() == ReceiverIR::Received)
@@ -160,231 +86,13 @@
{
case 0x45:
// ch- button (P value up)
- button = 0x1C;
pval += 0.01;
pval = fabs(pval);
update_display();
-// pc.printf("pval:%f\r\n", pval);
- wait(0.3);
- break;
-
- case 0x46:
- // ch common button (I value up)
- button = 0x1C;
- ival += 0.00001;
- ival = fabs(ival);
- update_display() ;
-// pc.printf("ival:%f\r\n", ival);
- wait(0.3);
- break;
-
- case 0x47:
- // ch+ button (D value up)
- button = 0x1C;
- dval += 0.05;
- dval = fabs(dval);
- update_display();
- wait(0.3);
- break;
-
- case 0x44:
- // prev button (P value down)
- button = 0x1C;
- pval -= 0.01;
- pval = fabs(pval);
- update_display();
- wait(0.3);
- break;
-
- case 0x40:
- // next button (I value down)
- button = 0x1C;
- ival -= 0.00001;
- ival = fabs(ival);
- update_display();
- wait(0.3);
- button = 0x1C;
- break;
-
- case 0x43:
- // play/pause button (D value down)
- button = 0x1C;
- dval -= 0.05;
- dval = fabs(dval);
- update_display();
- wait(0.3);
- button = 0x1C;
- break;
-
- case 0x07:
- // vol- button (velocity down)
- button = 0x1C;
- vel -= 5;
- update_display();
- wait(0.3);
- button = 0x1C;
- break;
-
- case 0x15:
- // vol+ button (velocity up)
- button = 0x1C;
- vel += 5;
- update_display();
wait(0.3);
button = 0x1C;
break;
-
- case 0x16:
- // 0 button (Reset the error variables)
- int_err = 0;
- err = 0;
- prev_err = 0;
- wait(1);
- button = 0x1C;
- break;
-
- case 0x19:
- // 100+ button (Calibrate)
- pc.printf("calibrate!\r\n");
- trs.calibrate();
- pc.printf("calibrate done\r\n");
- wait(0.2);
- button = 0x1C;
- break;
-
- case 0x18:
- // 2 button (move forward)
- motorDriver.user_forward(0.3,0.3);
- trs.calibrate();
- wait(0.1);
- button = 0x1C;
- break;
-
- case 0x52:
- // 8 button (move backward)
- motorDriver.user_backward(0.3,0.3);
- trs.calibrate();
- wait(0.1);
- button = 0x1C;
- break;
-
- case 0x5A:
- // 6 button (clockwise turn)
- motorDriver.user_right(0.3,0.1);
- wait(0.1);
- button = 0x1C;
- break;
-
- case 0x08:
- // 4 button (counter clockwise turn)
- motorDriver.user_left(0.1,0.3);
- wait(0.1);
- button = 0x1C;
- break;
-
- case 0x1C:
- // 5 button (motor stop)
- motorDriver.stop();
- break;
-
- case 0x0D:
- // 200+ button (line tracer)
- t=0;
- timer.reset();
- timer.start();
- ultra.setMode(false); // have updates every .1 seconds and try only once.
-
- while(1)
- {
- t=timer.read();
- position=trs.readLine(sensorValues,0);
- ultra.trig();
- dist = ultra.getDistance();
- pc.printf("dist:%d\r\n", dist);
- if(dist<=21){
- motorDriver.user_left(0.2,0.2);
- wait(0.1);
- while(1){
-
- // pc.printf("distance: %d\r\n", dist);
- // pc.printf("position: %d\r\n", position);
- position=trs.readLine(sensorValues,0);
- if(position > 2000)
- {
- int_err = 0;
- err = 0;
- prev_err = 0;
- break;
- }
- }
- }
-
- err=(int)position-2000; // error>0 --> right, error<0 --> left
- int_err+=err;
-
- const float kp = pval;
- const float kd = dval;
- const float ki = ival;
-
- float power_difference = kp*err + kd*(err-prev_err) + ki*int_err; //+derivative; //error를 누적하는 것
- prev_err = err;
-
- const float maximum=vel;
- if(power_difference > maximum)
- power_difference = maximum;
- if(power_difference < -maximum)
- power_difference = -maximum;
-
- if(power_difference<0)
- motorDriver.user_forward((maximum)/255,(maximum+power_difference)/255);
- else
- motorDriver.user_forward((maximum-power_difference)/255,(maximum)/255);
-
- pc.printf("position value: %d\r\n", position);
-
- //pc.printf("cnt: %d\r\n", cnt);
- for(int i=0;i<5;i++){
- pc.printf("%d\r\n",sensorValues[i]);
- }
- if((sensorValues[0] > 650) && (sensorValues[1]>650) && (sensorValues[2]>650) && (sensorValues[3]>650) && (sensorValues[4]>650))
- { // 5 IR sensor are on black
- timer.stop();
- t=timer.read();
- update_display();
- motorDriver.stop();
- for(int z=48;z>=0;z=z-4){
- ws.write_offsets(px.getBuf(),z,z,z);
- wait(0.1);
- }
-
- button = 0x1C;
- break;
- }
- }
- break;
-
- case 0x42:
- // 7 button (read sensor values)
- position=trs.readLine(sensorValues,0);
- for(int i=0; i<5; i++) {
- pc.printf("%d\r\n",sensorValues[i]);
- }
- pc.printf("done!\r\n");
- button = 0x1C;
- break;
-
-
- case 0x4A:
- // 9 button (read position)
- int j = 0;
- while(j!=100)
- {
- j++;
- position=trs.readLine(sensorValues,0);
- pc.printf("%d\r\n",position);
- }
- button = 0x1C;
- break;
+
default:
// wrong button
pc.printf("wrong button!\r\n");