Marcus Pereira
Leader Robot
Dependencies: Motor PID Servo mbed-rtos mbed
Revision 0:35cf52223527, committed 2016-12-11
- Comitter:
- mpereira30
- Date:
- Sun Dec 11 13:37:12 2016 +0000
- Commit message:
- Final Project Leader Robot
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Sun Dec 11 13:37:12 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/mpereira30/code/Motor/#d76429c5c280
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Sun Dec 11 13:37:12 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Sun Dec 11 13:37:12 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Dec 11 13:37:12 2016 +0000
@@ -0,0 +1,316 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "Motor.h"
+#include "PID.h"
+#include "rtos.h"
+#include <stdlib.h>
+
+#define PI 3.1415926
+#define threshold 27
+
+
+//objects
+Motor rightMotor(p24, p28, p27); // pwm, fwd(in1), rev(in2) Motor B (right wheel)
+Motor leftMotor(p22, p29, p30); // pwm, fwd(in1), rev(in2) Motor A (left wheel)
+Timer timer;
+AnalogIn ain(p20);
+AnalogIn ainL(p18);
+AnalogIn ainR(p19);
+Servo myservo(p26);
+Serial pc(USBTX, USBRX);
+Serial xbee1(p13, p14);
+Mutex wheelMutex;
+
+
+//globals
+float max_angle = 0.0;
+int leftPrevPulses = 0;
+int leftActPulses=0; //pulses from left motor
+float leftVelocity = 0.0; //The velocity of the left wheel in pulses per second
+int rightPrevPulses = 0;
+int rightActPulses=0; //pulses from right motor
+float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second
+int turnDistance = 0; //Number of pulses to travel.
+int distanceR = 0; // in mm
+int distanceL = 0; // in mm
+int distanceRold = 0; // in mm
+int distanceLold = 0; // in mm
+float X = 0.0;
+float Y = 0.0;
+float inc_X = 1.0;
+float inc_Y = 0.0;
+float cur_dir = 0.0;
+bool go;
+int DistancesR[4];
+int DistancesL[4];
+
+
+//functions
+void lookaround(void);
+void go_fwd(void);
+void turn_R(float);
+void turn_L(float);
+void stop(void);
+void distR(int);
+void distL(int);
+int distTransform(float input);
+void tunePID(void);
+float find_avg(float a[5]);
+float to_Rad(float);
+void go_fwd_thread(void const *argument);
+void grab_init_direct(void);
+
+class Counter { //interrupt driven rotation counter to be used with the feedback control
+ public:
+ Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
+ _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
+ }
+
+ void increment() {
+ _count++;
+ }
+
+ int read() {
+ return _count;
+ }
+
+private:
+ InterruptIn _interrupt;
+ volatile int _count;
+};
+
+Counter leftPulses(p9), rightPulses (p10);
+//PID leftPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
+//PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
+
+PID leftPid(0.462, 0.1, 0.2, 0.01); //Kc, Ti, Td
+PID rightPid(0.462, 0.1, 0.1, 0.01); //Kc, Ti, Td
+
+int main()
+{
+ tunePID();
+ xbee1.baud(115200);
+ myservo = 0.65;
+ leftPid.setSetPoint(975); //set velocity goals for PID
+ rightPid.setSetPoint(990);
+ Thread fwdThread(go_fwd_thread);
+ grab_init_direct();
+ go = true;
+ float middle[5];
+ float left[5];
+ float right[5];
+
+ while(1){
+ float mid_avg = 0.0;
+ float left_avg = 0.0;
+ float right_avg = 0.0;
+ timer.start();
+ while (mid_avg <0.4 && left_avg <0.7 && right_avg <0.4)
+ {
+ middle[0] = ain;
+ middle[1] = ain;
+ middle[2] = ain;
+ middle[3] = ain;
+ middle[4] = ain;
+ mid_avg = find_avg(middle);
+
+ left[0] = ainL;
+ left[1] = ainL;
+ left[2] = ainL;
+ left[3] = ainL;
+ left[4] = ainL;
+ left_avg = find_avg(left);
+
+ right[0] = ainR;
+ right[1] = ainR;
+ right[2] = ainR;
+ right[3] = ainR;
+ right[4] = ainR;
+ right_avg = find_avg(right);
+
+ X = X + inc_X;
+ Y = Y + inc_Y;
+ xbee1.printf("+\n");
+ xbee1.printf("%.2f\n%.2f\n" ,X ,Y);
+ }
+ go = false;
+ stop();
+ xbee1.printf("*\n");
+ xbee1.printf("%4d",timer.read_ms());
+ timer.stop();
+ timer.reset();
+ lookaround();
+ //fwdThread.signal_set(0x1);
+
+ if (max_angle >= 45.0)
+ {
+ turn_R((max_angle - 45.0));
+ cur_dir = cur_dir - (max_angle - 45.0);
+ inc_X = cos(to_Rad(cur_dir));
+ inc_Y = sin(to_Rad(cur_dir));
+ xbee1.printf("R\n");
+ xbee1.printf("%2.1f",(max_angle - 45.0));
+ }
+ else if (max_angle < 45.0)
+ {
+ turn_L((45.0 - max_angle));
+ cur_dir = cur_dir + (45.0 - max_angle);
+ inc_X = cos(to_Rad(cur_dir));
+ inc_Y = sin(to_Rad(cur_dir));
+ xbee1.printf("L\n");
+ xbee1.printf("%2.1f",(45.0 - max_angle));
+ }
+
+ myservo = 0.65;
+ stop();
+ wait(0.1);
+ go = true;
+
+ } // End of while(1)loop - main thread
+}// End of main()
+
+void grab_init_direct(void)
+{
+ char initdirect[4];
+ int Count = 0;
+ initdirect[Count] = xbee1.getc();
+ while(initdirect[Count] != '+')
+ {
+ Count++;
+ initdirect[Count] = xbee1.getc();
+ }
+
+ char initdirect2[Count];
+ for(int i=0; i<Count; i++)
+ {
+ initdirect2[i] = initdirect[i];
+ }
+
+ float init_turn = atof(initdirect2);
+ cur_dir = init_turn;
+ turn_R(init_turn);
+
+}
+
+float find_avg(float a[5])
+{
+ return ((a[0] + a[1] + a[2] + a[3] + a[4])/5.0);
+}
+
+void tunePID(void){
+ leftPid.setInputLimits(0, 3000);
+ leftPid.setOutputLimits(0.0, 0.5);
+ leftPid.setMode(AUTO_MODE);
+ rightPid.setInputLimits(0, 3000);
+ //rightPid.setOutputLimits(0.0, 0.465);
+ rightPid.setOutputLimits(0.0, 0.48);
+ rightPid.setMode(AUTO_MODE);
+}
+
+
+void stop(void)
+{
+ wheelMutex.lock();
+
+ leftMotor.speed(0.0);
+ rightMotor.speed(0.0);
+
+ wheelMutex.unlock();
+}
+
+
+void turn_R(float deg)
+{
+ wheelMutex.lock();
+
+ leftActPulses=leftPulses.read();
+ rightActPulses=rightPulses.read();
+ turnDistance=rightActPulses+(int)deg;
+ while (rightActPulses<turnDistance) { //turn approximately half a revolution
+ leftMotor.speed(0.4); //rotate to the right
+ rightMotor.speed(-0.4); //open loop, because the PID class can't handle negative values
+ leftActPulses=leftPulses.read();
+ rightActPulses=rightPulses.read();
+ wait(0.005);
+ }
+
+ wheelMutex.unlock();
+}
+
+void turn_L(float deg)
+{
+
+ wheelMutex.lock();
+
+ leftActPulses=leftPulses.read();
+ rightActPulses=rightPulses.read();
+ turnDistance=leftActPulses+(int)deg;
+ while (leftActPulses<turnDistance) { //turn approximately half a revolution
+ leftMotor.speed(-0.4); //rotate to the right
+ rightMotor.speed(0.4); //open loop, because the PID class can't handle negative values
+ leftActPulses=leftPulses.read();
+ rightActPulses=rightPulses.read();
+ wait(0.005);
+ }
+
+ wheelMutex.unlock();
+}
+
+void lookaround(void) //returns a value between 0 and 90 that represents the best path in the cone of observation
+{
+ float max;
+ max_angle = 0.0;
+ max = ain;
+ for(float p=0; p<1.0; p += 0.05)
+ {
+ myservo = p;
+ if (max > ain)
+ {
+ max = ain;
+ max_angle = p;
+ }
+
+ //wait(0.25);
+ wait(0.1);
+ }
+
+ if (max>0.45){
+ turn_L(180.0);
+ myservo = 0.65;
+ wait(0.5);
+ }
+ else{
+ myservo = max_angle;
+ wait(0.5);
+ max_angle = max_angle*90.0 - 13.5;
+ //max_angle = max_angle*90.0;
+ }
+
+}
+
+float to_Rad(float deg)
+{
+ return deg * ((PI)/180);
+}
+
+void go_fwd_thread(void const *argument) {
+ while(1){
+ if(!go) {Thread::yield();}
+ while (go) {
+ wheelMutex.lock();
+
+ leftActPulses=leftPulses.read();
+ leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
+ leftPrevPulses = leftActPulses;
+ rightActPulses=rightPulses.read();
+ rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
+ rightPrevPulses = rightActPulses;
+ leftPid.setProcessValue(fabs(leftVelocity));
+ leftMotor.speed(leftPid.compute()+0.0075);
+ rightPid.setProcessValue(fabs(rightVelocity));
+ rightMotor.speed(rightPid.compute());
+
+ wheelMutex.unlock();
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Sun Dec 11 13:37:12 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#b744dfee1cf2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Dec 11 13:37:12 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/d75b3fe1f5cb \ No newline at end of file