![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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