This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Revision 65:4709ff6c753c, committed 2013-04-14
- Comitter:
- rsavitski
- Date:
- Sun Apr 14 17:57:12 2013 +0000
- Parent:
- 59:63609922579c
- Child:
- 67:be3ea5450cc7
- Child:
- 68:662164480f60
- Commit message:
- rewrote arm code, pushes top layer with uber paddle
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Actuators/Arms/Arm.cpp Sun Apr 14 17:57:12 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 Sat Apr 13 22:42:01 2013 +0000 +++ b/Actuators/Arms/Arm.h Sun Apr 14 17:57:12 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 Sat Apr 13 22:42:01 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 Sat Apr 13 22:42:01 2013 +0000 +++ b/Processes/AI/ai.cpp Sun Apr 14 17:57:12 2013 +0000 @@ -4,6 +4,7 @@ #include "motion.h" #include "Colour.h" #include "supportfuncs.h" +#include "Arm.h" namespace AI @@ -17,20 +18,26 @@ current_waypoint.y = 1.85; current_waypoint.theta = PI; current_waypoint.pos_threshold = 0.01; - current_waypoint.angle_threshold = 0.02*PI; + current_waypoint.angle_threshold = 0.01*PI; motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); 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.03; 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) + if (motion::checkMotionStatus() && c_upper.getColour()==RED) { + //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);
--- a/Processes/Motion/motion.cpp Sat Apr 13 22:42:01 2013 +0000 +++ b/Processes/Motion/motion.cpp Sun Apr 14 17:57:12 2013 +0000 @@ -103,9 +103,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();
--- a/Processes/Printing/Printing.h Sat Apr 13 22:42:01 2013 +0000 +++ b/Processes/Printing/Printing.h Sun Apr 14 17:57:12 2013 +0000 @@ -1,7 +1,7 @@ // Eurobot13 Printing.h -//#define PRINTINGOFF +#define PRINTINGOFF #include "mbed.h" #include "rtos.h"
--- a/main.cpp Sat Apr 13 22:42:01 2013 +0000 +++ b/main.cpp Sun Apr 14 17:57:12 2013 +0000 @@ -39,8 +39,8 @@ //motorencodetest(); //motorencodetestline(); //motorsandservostest(); - armtest(); - while(1); + //armtest(); + //while(1); //motortestline(); //ledtest(); //phototransistortest(); @@ -245,22 +245,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.2); - wait(1); - white(1); - black(0.8); - 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); @@ -301,7 +353,7 @@ if (servoTimer.read() >= 9) servoTimer.reset(); } } - +*/ void motortestline() { MainMotor mleft(p24,p23), mright(p21,p22);