Anon Anon
Code
Revision 0:8adde5eaea5a, committed 2017-11-08
- Comitter:
- Showboo
- Date:
- Wed Nov 08 20:30:42 2017 +0000
- Commit message:
- Prev
Changed in this revision
diff -r 000000000000 -r 8adde5eaea5a actions.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/actions.h Wed Nov 08 20:30:42 2017 +0000
@@ -0,0 +1,75 @@
+#ifndef ACTIONS_H
+#define ACTIONS_H
+
+PwmOut mypwm(PWM_OUT);
+DigitalOut myled(LED1);
+//Left Motor
+PwmOut m1F(PC_7);
+PwmOut m1B(PB_10);
+//Right Motor
+PwmOut m2F(PA_7);
+PwmOut m2B(PB_6);
+//Left Encoder
+AnalogIn lFront(PB_3);
+AnalogIn lBack(PA_15);
+//Right Encoder
+AnalogIn rFront(PA_1);
+AnalogIn rBack(PC_14);
+
+Timer time;
+
+struct pid { //Note that because we have two different types of distance sensors (Andrew's works a little differently than Jeffrey's we should have two different errors. To stay straight though we can just use one side right?)
+ pid(){
+ integral = 0.08f;
+ prev = 0.0f;
+ kp = 1.0f; //the ks should be negative to counteract error
+ ki = 0.0f;
+ kd = 0.8f;
+ }
+ float integral;
+ int prev;
+ float kp; //the ks should be negative to counteract error
+ float ki;
+ float kd;
+};
+
+void resetPid(struct pid *e) {
+ e->integral = 0.0f;
+ e->prev = 0.0f;
+}
+
+float getFix(struct pid *e, float error) {
+ //float d = (error - e->prev)/DT;
+ e->integral += error * DT;
+ e->prev = error;
+}
+
+
+void turnleft_loop(float lencoder, float rencoder){ //4096 per turn of wheel
+ while(lencoder < 12861.0f){
+ float error = lencoder - 12861.0f;
+ float delta_motor = getFix(lturn, error);
+ m1F.period(0.01f);
+ m1F = m1basespeed + delta_motor;
+ }
+ while(rencoder < 12861f){
+ float error = rencoder - 12861f;
+ float delta_motor = getFix(rturn, error);
+ m2B.period(0.01f);
+ m2B = m2basespeed + delta_motor;
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 8adde5eaea5a main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Nov 08 20:30:42 2017 +0000
@@ -0,0 +1,30 @@
+#include "mbed.h"
+#include "actions.h"
+
+int main() {
+ pid lman, rman;
+ lman.kp = .5;
+ lman.ki = .1;
+ lman.kd = .4;
+ rman.kp = .5;
+ rman.ki = .1;
+ rman.kd = .4;
+ //INITIALIZATION
+ while(1) {
+ float left_encoder = lfront.read();
+ float right_encoder = rfront.read();
+ float bl = lback.read();
+ float br = rback.read();
+ printf("Left Encoder: %.2f %%\n", lfront.read());
+ printf("Right Encoder: %.2f %%\n", rfront.read());
+ printf("BL Encoder: %.2f %%\n", lback.read());
+ printf("BR Encoder: %.2f %%\n", rback.read());
+
+ float error = left_encoder - right_encoder; //Can be pos or neg
+ if(error == 0){
+ resetpid(lman);
+ resetpid(rman);
+ }
+ float adjust_r = getFix(rman, error); //Only right wheel adjusted for this example, we can change this later / make it more complicated
+ }
+}
diff -r 000000000000 -r 8adde5eaea5a mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Nov 08 20:30:42 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/fb8e0ae1cceb \ No newline at end of file