Mitchell Jones
Dependencies: Sabertooth mbed pixy
Revision 0:1b62d4a3444b, committed 2015-11-11
- Comitter:
- mitchelljones
- Date:
- Wed Nov 11 03:41:19 2015 +0000
- Commit message:
- Camera
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sabertooth.lib Wed Nov 11 03:41:19 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/bediyap/code/Sabertooth/#8ad928968f9e
--- /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
+ }
+}
+
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Nov 11 03:41:19 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9296ab0bfc11 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pixy.lib Wed Nov 11 03:41:19 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/LucasUIUC/code/pixy/#f8b6497870d3