Diff: main.cpp

Revision:

4:4afa448c9cce

Parent:

3:87a5122682fa

Child:

6:b0e160c51013

--- a/main.cpp	Wed Nov 02 13:13:13 2016 +0000
+++ b/main.cpp	Thu Nov 03 13:06:16 2016 +0000
@@ -1,11 +1,13 @@
 #include "mbed.h"
 #include "TFC.h"
+#include "XBEE.h"
 #define CAM_THRESHOLD 128
  
  
 DigitalOut myled(LED1);
 //Serial pc(USBTX,USBRX);
 Serial pc(PTD3,PTD2);
+XBEE xb(&pc);
 
 char curr_line[128];
 uint8_t curr_left;
@@ -15,12 +17,16 @@
 uint8_t left;
 
 Timer t;
+
+char curr_cmd = 0;
+
+float speed = 0.3;
  
 int main() {
     TFC_Init();
     TFC_InitServos(0.00052,0.00122,0.02);
-    TFC_HBRIDGE_ENABLE;
-    TFC_SetMotorPWM(0.3,0.3);
+    //TFC_HBRIDGE_ENABLE;
+    //TFC_SetMotorPWM(0.3,0.3);
     
     
     pc.baud(57600);
@@ -31,9 +37,9 @@
     float output;
       //tunable variables
     float Kp, Ki,Kd;
-        Kp=0.5;
-    Ki=0.1;
-    Kd=0;
+        Kp=0.8;
+    Ki=0.6;
+    Kd=0.0;
     myled = 0;// Test
         float dt=0;
     p_error=0;
@@ -48,11 +54,63 @@
     uint8_t i = 0;
     while(1) {
         
+        if(curr_cmd != 0) {
+            switch(curr_cmd) {
+                case 'A':
+                    if(xb.cBuffer->available() >= 3) {
+                        char p = xb.cBuffer->read();
+                        char i = xb.cBuffer->read();
+                        char d = xb.cBuffer->read();
+                        Kp = p/256.0f;
+                        Ki = i/256.0f;
+                        Kd = d/256.0f;
+                        pc.putc('E');
+                        pc.printf("pid change\0");
+                        curr_cmd = 0;        
+                    }    
+                break;
+                
+                case 'F':
+                    if(xb.cBuffer->available() >= 2) {
+                        char a = xb.cBuffer->read();
+                        char b = xb.cBuffer->read();
+                        short s = (a << 8) & b;
+                        
+                        speed = s/65536.0f;
+                        pc.putc('E');
+                        pc.printf("speed change\0");
+                        curr_cmd = 0;   
+                        
+                    }
+                break;
+                  
+                default:
+                break; 
+            }
+        }
+        
+        if(xb.cBuffer->available() > 0) {
+            char cmd = xb.cBuffer->read();
+            if(cmd == 'D') {
+                TFC_InitServos(0.00052,0.00122,0.02);
+                TFC_HBRIDGE_ENABLE;
+                TFC_SetMotorPWM(speed,speed);   
+            } else if (cmd == 'C') {
+                TFC_SetMotorPWM(0.0,0.0);
+                TFC_HBRIDGE_DISABLE;
+            } else if(cmd == 'A') {
+                curr_cmd = 'A';
+            } else if(cmd == 'F') {
+                curr_cmd = 'F';    
+            }
+            
+        }
+        
         //If we have an image ready
         if(TFC_LineScanImageReady>0) {       
             left = 0;
             right = 0;
-            for(i = 63; i >= 0; i--) {
+            for(i = 63; i > 0; i--) {
                 curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
                 if(curr_left < CAM_THRESHOLD) {
                     left = i;
@@ -91,11 +149,9 @@
     else
  {  
     while(1){
-        myled = 1;
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
-        integral=0;
+       pc.putc('E');
+       pc.printf("pid has borked :( \0");
+       wait(0.2f);
    }   
    }