Revision 67:be3ea5450cc7, committed 2013-04-14

Comitter:

madcowswe

Date:

Sun Apr 14 18:47:17 2013 +0000

Parent:

66:f1d75e51398d

Parent:

65:4709ff6c753c

Child:

69:4b7bb92916da

Commit message:

Tuned merge. Testing needed

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuators/Arms/Arm.cpp	Sun Apr 14 18:47:17 2013 +0000
@@ -0,0 +1,9 @@
+#include "Arm.h"
+
+namespace arm
+{
+
+Arm lower_arm(p25, 0.05, 0.9, 2.0);
+Arm upper_arm(p26, 0.05, 0.6, 2.5);
+
+} //namespace
\ No newline at end of file

--- a/Actuators/Arms/Arm.h	Sun Apr 14 17:22:20 2013 +0000
+++ b/Actuators/Arms/Arm.h	Sun Apr 14 18:47:17 2013 +0000
@@ -1,69 +1,44 @@
-
-// Eurobot13 WhiteArm.h
+#ifndef EUROBOT_ACTUATORS_ARMS_ARM_H_
+#define EUROBOT_ACTUATORS_ARMS_ARM_H_
 
 #include "mbed.h"
-#include "Servo.h"
 
-class Arm : public Servo
+namespace arm
 {
-private:
-    bool updirn;
+
+class Arm;
 
+extern Arm lower_arm;
+extern Arm upper_arm;
+
+
+class Arm 
+{
 public:
-    Arm ( PinName yellowPWM
-        , bool upflip = false
-        , float range = 0.0005, float degrees = 45.0
-        ) 
-        : Servo(yellowPWM) 
-        {
-        calibrate(range, degrees);
-        updirn = upflip;
+    Arm (PwmOut pwm_in, float period_in, float min_pos_in, float max_pos_in)
+       : pwm_(pwm_in), period_(period_in), min_pos_(min_pos_in), max_pos_(max_pos_in)
+    {
+        pwm_.period(period_);
     }
 
-    void operator()(float in) {
-        write(in);
-    }
-
-    void clockwise() { // full lock clockwise
-        write(updirn?1:0);
-    }
-
-    void anticlockwise() { // full lock anticlockwise
-        write(updirn?0:1);
+    void go_up()
+    {
+        pwm_.pulsewidth_us(max_pos_*1000);
     }
     
-    virtual void halt() { // servo applies no force
-        //DigitalOut myled(LED3);
-        //myled = 1;
-        _pwm = 0;
-    }
-};
-
-
-/*
-class Servo{
-    private:
-    PwmOut PWM;
-
-    public:
-    Servo(PinName pin1) : PWM(pin1){
+    void go_down()
+    {
+        pwm_.pulsewidth_us(min_pos_*1000);
     }
 
-    void operator()(float in){
-        PWM = in;
-    }
-
-    void clockwise() { // full lock clockwise
-        PWM = .135;
-    }
-
-    void anticlockwise() { // full lock anticlockwise
-        PWM = .025;
-    }
-
-    void relax() { // servo applies no force
-        PWM = 0;
-    }
+private:
+    PwmOut pwm_;
+    float period_;
+    float min_pos_;
+    float max_pos_;
 
 };
-*/
\ No newline at end of file
+
+} //namespace
+
+#endif
\ No newline at end of file

--- a/Actuators/Arms/Servo.lib	Sun Apr 14 17:22:20 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- a/Processes/AI/ai.cpp	Sun Apr 14 17:22:20 2013 +0000
+++ b/Processes/AI/ai.cpp	Sun Apr 14 18:47:17 2013 +0000
@@ -4,6 +4,7 @@
 #include "motion.h"
 #include "Colour.h"
 #include "supportfuncs.h"
+#include "Arm.h"
 
 
 namespace AI
@@ -15,8 +16,8 @@
 
     current_waypoint.x = 2.2;
     current_waypoint.y = 1.85;
-    current_waypoint.theta = PI;
-    current_waypoint.pos_threshold = 0.01;
+    current_waypoint.theta = (-3.0f/4.0f)*PI;
+    current_waypoint.pos_threshold = 0.03;
     current_waypoint.angle_threshold = 0.02*PI;
 
     motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
@@ -25,14 +26,20 @@
     Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
     Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
 
-    float r = 0.61+0.02;
+    float r = 0.61+0.02+0.01;
 
     bool firstavoidstop = 1;
     for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
         motion::waypoint_flag_mutex.lock();
-        if (motion::checkMotionStatus() && (c_lower.getColour()==RED || firstavoidstop))
+        if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop))
         {
+            //temphack!!!!!
+            Thread::wait(1000);
+            arm::upper_arm.go_down();
+            Thread::wait(2000);
+            arm::upper_arm.go_up();
+            
             phi -= 22.5/180*PI;
             current_waypoint.x = 1.5-r*cos(phi);
             current_waypoint.y = 2-r*sin(phi);
@@ -41,6 +48,9 @@
             //arm offset
             current_waypoint.x += 0.0425*cos(current_waypoint.theta);
             current_waypoint.y += 0.0425*sin(current_waypoint.theta);
+            
+            current_waypoint.pos_threshold = 0.01;
+            current_waypoint.angle_threshold = 0.01*PI;
 
             motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
             if (firstavoidstop){

--- a/Processes/Motion/motion.cpp	Sun Apr 14 17:22:20 2013 +0000
+++ b/Processes/Motion/motion.cpp	Sun Apr 14 18:47:17 2013 +0000
@@ -107,9 +107,7 @@
             wp_a_reached = true;
         }
     } 
-    waypoint_flag_mutex.unlock();
-        
-    waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
+     // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
     if (wp_d_reached && wp_a_reached)
     {
         setWaypointReached();
@@ -117,7 +115,7 @@
     waypoint_flag_mutex.unlock();
     
     // angular velocity controller
-    const float p_gain_av = 0.5; //TODO: tune
+    const float p_gain_av = 0.7; //TODO: tune
     
     const float max_av = 0.5; // radians per sec //TODO: tune
     
@@ -132,11 +130,11 @@
     
     
     // forward velocity controller
-    const float p_gain_fv = 0.5; //TODO: tune
+    const float p_gain_fv = 0.7; //TODO: tune
     
-    float max_fv = 0.3; // meters per sec //TODO: tune
+    float max_fv = 0.2; // meters per sec //TODO: tune
     float max_fv_reverse = 0.05; //TODO: tune
-    const float angle_envelope_exponent = 32.0;//512; //8.0; //TODO: tune
+    const float angle_envelope_exponent = 32;//512; //8.0; //TODO: tune
     
     // control, distance_err in meters
     float forward_v = p_gain_fv * distance_err;

--- a/Processes/Printing/Printing.h	Sun Apr 14 17:22:20 2013 +0000
+++ b/Processes/Printing/Printing.h	Sun Apr 14 18:47:17 2013 +0000
@@ -1,7 +1,7 @@
 
 // Eurobot13 Printing.h
 
-//#define PRINTINGOFF
+#define PRINTINGOFF
 
 #include "mbed.h"
 #include "rtos.h"

--- a/main.cpp	Sun Apr 14 17:22:20 2013 +0000
+++ b/main.cpp	Sun Apr 14 18:47:17 2013 +0000
@@ -40,6 +40,7 @@
     //motorencodetestline();
     //motorsandservostest();
     //armtest();
+    //while(1);
     //motortestline();
     //ledtest();
     //phototransistortest();
@@ -272,22 +273,74 @@
     }
 }
 */
+#ifdef AGDHGADSYIGYJDGA
+PwmOut white(p26);
+PwmOut black(p25);
 
-
+void armtest() 
+{    
+    
+    white.period(0.05);
+    black.period(0.05);
+    
+  /*  float f=1;
+    for (f=1; f<3; f+=0.1)
+    { 
+        black.pulsewidth_us(f*1000);
+        wait(1);
+        printf("%f\r\n", f);
+    }
+    for (f=2; f>0; f-=0.1)
+    { 
+        black.pulsewidth_us(f*1000);
+        wait(1);
+        printf("%f\r\n", f);
+    }*/
+    
+    
+    for(;;)
+    {
+        black.pulsewidth_us(2.0*1000);
+        wait(2);
+        black.pulsewidth_us(0.9*1000);//1.2
+        wait(2);
+    }
+    
+    // white works
+    /*for(;;)
+    {
+        white.pulsewidth_us(0.6*1000);
+        wait(2);
+        white.pulsewidth_us(2.5*1000);
+        wait(2);
+    }*/
+    
+   /* while(1) //2.5 -> //0.6
+    {
+        white.pulsewidth_us(int(f*1000));
+        printf("%f\r\n", f);
+        f-=0.1;
+        wait(1);
+    }*/
+}
+#endif
+#ifdef FSDHGFSJDF
 void armtest()
 {
-    Arm white(p26), black(p25, false, 0.0005, 180);
-    while(true) {
-        white(0);
-        black(0);
-        wait(1);
-        white(1);
-        black(1);
-        wait(1);
+    Arm lower_arm(p25, 0.05, 0.9, 2.0);
+    Arm upper_arm(p26, 0.05, 0.6, 2.5);
+    
+    while(1)
+    {
+        upper_arm.go_up();
+        wait(2);
+        upper_arm.go_down();
+        wait(2);
     }
 }
-
-
+#endif
+void armtest(){}
+/*
 void motorsandservostest()
 {
     Encoder Eleft(p27, p28), Eright(p30, p29);
@@ -328,7 +381,7 @@
         if (servoTimer.read() >= 9) servoTimer.reset();
     }
 }
-
+*/
 void motortestline()
 {
     MainMotor mleft(p24,p23), mright(p21,p22);