Mitchell Jones / Mbed 2 deprecated Camera_Gimbal_Tracking

Dependencies:   Sabertooth mbed pixy

main.cpp@0:1b62d4a3444b, 2015-11-11 (annotated)

Committer:
mitchelljones
Date:
Wed Nov 11 03:41:19 2015 +0000
Revision:
0:1b62d4a3444b
Camera

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mitchelljones 0:1b62d4a3444b 1 #include "mbed.h"
mitchelljones 0:1b62d4a3444b 2 #include "Sabertooth.h"
mitchelljones 0:1b62d4a3444b 3 #include "Pixy.h"
mitchelljones 0:1b62d4a3444b 4 #include <cstdlib>
mitchelljones 0:1b62d4a3444b 5 #include <ros.h>
mitchelljones 0:1b62d4a3444b 6 #include <std_msgs/Int32.h>
mitchelljones 0:1b62d4a3444b 7
mitchelljones 0:1b62d4a3444b 8 #define X_CENTER 160
mitchelljones 0:1b62d4a3444b 9 #define Y_CENTER 100
mitchelljones 0:1b62d4a3444b 10
mitchelljones 0:1b62d4a3444b 11 float kp = 0.2;
mitchelljones 0:1b62d4a3444b 12 float ki = 0.001;
mitchelljones 0:1b62d4a3444b 13 float kd = 0.01;
mitchelljones 0:1b62d4a3444b 14 float leftAngle = 0.026;
mitchelljones 0:1b62d4a3444b 15 float rightAngle = 0.745;
mitchelljones 0:1b62d4a3444b 16
mitchelljones 0:1b62d4a3444b 17 float camAngle; // camera gimbal angle
mitchelljones 0:1b62d4a3444b 18 float desAngle = 100; // Center angle
mitchelljones 0:1b62d4a3444b 19 float indx = 0.5; // angle step
mitchelljones 0:1b62d4a3444b 20 int dir = 1; // direction of travel
mitchelljones 0:1b62d4a3444b 21
mitchelljones 0:1b62d4a3444b 22 float prevError; // for angle error derivative
mitchelljones 0:1b62d4a3444b 23 float angleError; // angle error
mitchelljones 0:1b62d4a3444b 24 float ddtAngleError; // derivative of angle error
mitchelljones 0:1b62d4a3444b 25 float intAngleError = 0; // integral of angle error
mitchelljones 0:1b62d4a3444b 26 float w; // motor speed
mitchelljones 0:1b62d4a3444b 27 float camAngle_dot = 0;
mitchelljones 0:1b62d4a3444b 28 float prev_cam_angle = 0;
mitchelljones 0:1b62d4a3444b 29
mitchelljones 0:1b62d4a3444b 30 Ticker loop;
mitchelljones 0:1b62d4a3444b 31 AnalogIn pot(p20);
mitchelljones 0:1b62d4a3444b 32 //DigitalOut myled(LED1);
mitchelljones 0:1b62d4a3444b 33 Serial pc(USBTX, USBRX);
mitchelljones 0:1b62d4a3444b 34 Sabertooth saber(p9,129,9600);
mitchelljones 0:1b62d4a3444b 35 Pixy pixy(Pixy::SPI, D11, D12, D13);
mitchelljones 0:1b62d4a3444b 36
mitchelljones 0:1b62d4a3444b 37
mitchelljones 0:1b62d4a3444b 38 void PID() {
mitchelljones 0:1b62d4a3444b 39
mitchelljones 0:1b62d4a3444b 40 uint16_t blocks;
mitchelljones 0:1b62d4a3444b 41 blocks = pixy.getBlocks();
mitchelljones 0:1b62d4a3444b 42 if (blocks) {
mitchelljones 0:1b62d4a3444b 43 prevError = angleError; // for derivative term
mitchelljones 0:1b62d4a3444b 44 angleError = (float)(pixy.blocks[0].x - X_CENTER);
mitchelljones 0:1b62d4a3444b 45 ddtAngleError = angleError - prevError;
mitchelljones 0:1b62d4a3444b 46 /*if( (angleError < 2) && (angleError > -2) ) // dead zone
mitchelljones 0:1b62d4a3444b 47 {
mitchelljones 0:1b62d4a3444b 48 angleError = 0;
mitchelljones 0:1b62d4a3444b 49 }*/
mitchelljones 0:1b62d4a3444b 50 intAngleError=intAngleError+angleError;
mitchelljones 0:1b62d4a3444b 51 w = (kp*angleError + ki*intAngleError + kd*ddtAngleError)+ 64 ;
mitchelljones 0:1b62d4a3444b 52 if (w<10) {
mitchelljones 0:1b62d4a3444b 53 w=10;
mitchelljones 0:1b62d4a3444b 54 }
mitchelljones 0:1b62d4a3444b 55 if (w>117) {
mitchelljones 0:1b62d4a3444b 56 w=117;
mitchelljones 0:1b62d4a3444b 57 }
mitchelljones 0:1b62d4a3444b 58
mitchelljones 0:1b62d4a3444b 59 prev_cam_angle = camAngle;
mitchelljones 0:1b62d4a3444b 60 camAngle = pot.read()*200;
mitchelljones 0:1b62d4a3444b 61 camAngle_dot = (camAngle-prev_cam_angle)/0.01;
mitchelljones 0:1b62d4a3444b 62 }
mitchelljones 0:1b62d4a3444b 63 else{
mitchelljones 0:1b62d4a3444b 64 w=64;
mitchelljones 0:1b62d4a3444b 65 }
mitchelljones 0:1b62d4a3444b 66
mitchelljones 0:1b62d4a3444b 67
mitchelljones 0:1b62d4a3444b 68
mitchelljones 0:1b62d4a3444b 69 /*if (dir>=0) {
mitchelljones 0:1b62d4a3444b 70 desAngle=desAngle + indx;
mitchelljones 0:1b62d4a3444b 71 }
mitchelljones 0:1b62d4a3444b 72 else {
mitchelljones 0:1b62d4a3444b 73 desAngle=desAngle - indx;
mitchelljones 0:1b62d4a3444b 74 }
mitchelljones 0:1b62d4a3444b 75 if ((desAngle>180) || (desAngle<20)) {
mitchelljones 0:1b62d4a3444b 76 dir=(-1)*dir;
mitchelljones 0:1b62d4a3444b 77 }
mitchelljones 0:1b62d4a3444b 78
mitchelljones 0:1b62d4a3444b 79 camAngle = pot.read()*200;
mitchelljones 0:1b62d4a3444b 80 angleError = desAngle- camAngle;
mitchelljones 0:1b62d4a3444b 81 */
mitchelljones 0:1b62d4a3444b 82
mitchelljones 0:1b62d4a3444b 83
mitchelljones 0:1b62d4a3444b 84 //prevAngle=camAngle;
mitchelljones 0:1b62d4a3444b 85
mitchelljones 0:1b62d4a3444b 86 if (intAngleError > 1) {
mitchelljones 0:1b62d4a3444b 87 intAngleError = 1;
mitchelljones 0:1b62d4a3444b 88 }
mitchelljones 0:1b62d4a3444b 89 }
mitchelljones 0:1b62d4a3444b 90
mitchelljones 0:1b62d4a3444b 91
mitchelljones 0:1b62d4a3444b 92 void cmd(const std_msgs::Int32 &cmd_msg) {
mitchelljones 0:1b62d4a3444b 93 L = cmd_msg.data;
mitchelljones 0:1b62d4a3444b 94 }
mitchelljones 0:1b62d4a3444b 95
mitchelljones 0:1b62d4a3444b 96
mitchelljones 0:1b62d4a3444b 97 ros::NodeHandle nh(USBTX,USBRX);
mitchelljones 0:1b62d4a3444b 98 ros::Subscriber<std_msgs::Int32> sub("camera", cmd);
mitchelljones 0:1b62d4a3444b 99
mitchelljones 0:1b62d4a3444b 100 int main() {
mitchelljones 0:1b62d4a3444b 101 //wait(2);
mitchelljones 0:1b62d4a3444b 102 nh.initNode();
mitchelljones 0:1b62d4a3444b 103 nh.subscribe(sub);
mitchelljones 0:1b62d4a3444b 104 loop.attach(&PID, 0.01);
mitchelljones 0:1b62d4a3444b 105 while(1) {
mitchelljones 0:1b62d4a3444b 106 nh.spinOnce();
mitchelljones 0:1b62d4a3444b 107 pc.printf("%f\n",camAngle);
mitchelljones 0:1b62d4a3444b 108 //wait(0.5);
mitchelljones 0:1b62d4a3444b 109 saber.SetSpeedMotorA(w); // Between 0 and 127
mitchelljones 0:1b62d4a3444b 110 }
mitchelljones 0:1b62d4a3444b 111 }
mitchelljones 0:1b62d4a3444b 112
mitchelljones 0:1b62d4a3444b 113
mitchelljones 0:1b62d4a3444b 114
mitchelljones 0:1b62d4a3444b 115
mitchelljones 0:1b62d4a3444b 116