![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 |