Diff: main.cpp

Revision:

0:1b62d4a3444b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 11 03:41:19 2015 +0000
@@ -0,0 +1,116 @@
+#include "mbed.h"
+#include "Sabertooth.h"
+#include "Pixy.h"
+#include <cstdlib>
+#include <ros.h>
+#include <std_msgs/Int32.h>
+
+#define X_CENTER 160
+#define Y_CENTER 100
+
+float kp = 0.2;
+float ki = 0.001;
+float kd = 0.01;
+float leftAngle = 0.026;
+float rightAngle = 0.745;
+
+float camAngle;  // camera gimbal angle
+float desAngle = 100;  // Center angle
+float indx = 0.5;  // angle step
+int dir = 1; // direction of travel
+
+float prevError;  // for angle error derivative
+float angleError;  // angle error
+float ddtAngleError;  // derivative of angle error
+float intAngleError = 0;  // integral of angle error
+float w;  // motor speed
+float camAngle_dot = 0;
+float prev_cam_angle = 0;
+
+Ticker loop;
+AnalogIn pot(p20);
+//DigitalOut myled(LED1);
+Serial pc(USBTX, USBRX);
+Sabertooth saber(p9,129,9600);
+Pixy pixy(Pixy::SPI, D11, D12, D13);
+
+
+void PID() {
+    
+    uint16_t blocks;
+    blocks = pixy.getBlocks();
+    if (blocks) {
+        prevError = angleError;  // for derivative term
+        angleError =  (float)(pixy.blocks[0].x - X_CENTER);
+        ddtAngleError = angleError - prevError;
+        /*if( (angleError < 2) && (angleError > -2) )  // dead zone
+            {
+                angleError = 0;
+            }*/
+        intAngleError=intAngleError+angleError;
+        w = (kp*angleError + ki*intAngleError + kd*ddtAngleError)+ 64 ;
+        if (w<10) {
+            w=10;
+        }
+        if (w>117) {
+            w=117;
+        }
+        
+        prev_cam_angle = camAngle;
+        camAngle = pot.read()*200;
+        camAngle_dot = (camAngle-prev_cam_angle)/0.01;
+    }
+    else{
+        w=64;
+        }
+    
+    
+  
+    /*if (dir>=0) {
+        desAngle=desAngle + indx;
+    }
+    else {
+        desAngle=desAngle - indx;
+    }
+     if ((desAngle>180) || (desAngle<20)) {
+        dir=(-1)*dir;
+    }
+        
+    camAngle = pot.read()*200;
+    angleError = desAngle- camAngle;
+    */
+    
+        
+    //prevAngle=camAngle;
+        
+    if (intAngleError > 1) {
+        intAngleError = 1;
+        }   
+}
+
+
+void cmd(const std_msgs::Int32 &cmd_msg) {
+     L = cmd_msg.data;
+}
+
+
+ros::NodeHandle nh(USBTX,USBRX);
+ros::Subscriber<std_msgs::Int32> sub("camera", cmd);
+
+int main() {
+    //wait(2);
+    nh.initNode();
+    nh.subscribe(sub);
+    loop.attach(&PID, 0.01);
+    while(1) {
+        nh.spinOnce();
+        pc.printf("%f\n",camAngle);
+        //wait(0.5);
+        saber.SetSpeedMotorA(w);  // Between 0 and 127
+    }
+}
+
+
+
+
+