Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/AI/ai.cpp@76:532d9bc1d2aa, 2013-04-15 (annotated)

Committer:
rsavitski
Date:
Mon Apr 15 13:53:58 2013 +0000
Revision:
76:532d9bc1d2aa
Parent:
70:0da6ca845762
Child:
77:8d83a0c00e66
tuning gains with counterweights + cleanup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsavitski 30:791739422122 1 #include "ai.h"
rsavitski 55:0c8897da6b3a 2 #include "rtos.h"
rsavitski 55:0c8897da6b3a 3 #include "globals.h"
rsavitski 55:0c8897da6b3a 4 #include "motion.h"
rsavitski 55:0c8897da6b3a 5 #include "Colour.h"
rsavitski 55:0c8897da6b3a 6 #include "supportfuncs.h"
rsavitski 65:4709ff6c753c 7 #include "Arm.h"
rsavitski 55:0c8897da6b3a 8
rsavitski 69:4b7bb92916da 9 //TODO: after 2013, kill entire AI layer as it is hacked together. Rest is fine-ish
rsavitski 30:791739422122 10
rsavitski 30:791739422122 11 namespace AI
rsavitski 30:791739422122 12 {
rsavitski 30:791739422122 13
rsavitski 69:4b7bb92916da 14 bool delayed_done = true; //TODO: kill
rsavitski 69:4b7bb92916da 15
rsavitski 69:4b7bb92916da 16 struct delayed_struct //TODO: kill
rsavitski 69:4b7bb92916da 17 {
rsavitski 69:4b7bb92916da 18 osThreadId tid;
rsavitski 69:4b7bb92916da 19 Waypoint *wpptr;
rsavitski 69:4b7bb92916da 20 };
rsavitski 69:4b7bb92916da 21
rsavitski 76:532d9bc1d2aa 22 void raise_top_arm(const void *dummy);
rsavitski 76:532d9bc1d2aa 23 void raise_bottom_arm(const void *dummy);
rsavitski 70:0da6ca845762 24
rsavitski 69:4b7bb92916da 25 void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
rsavitski 69:4b7bb92916da 26
rsavitski 30:791739422122 27 void ailayer(void const *dummy)
rsavitski 30:791739422122 28 {
rsavitski 54:99d3158c9207 29 Waypoint current_waypoint;
rsavitski 69:4b7bb92916da 30
madcowswe 50:937e860f4621 31 Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
madcowswe 50:937e860f4621 32 Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
rsavitski 55:0c8897da6b3a 33
rsavitski 69:4b7bb92916da 34 // first waypoint for approach
rsavitski 54:99d3158c9207 35 current_waypoint.x = 2.2;
rsavitski 54:99d3158c9207 36 current_waypoint.y = 1.85;
madcowswe 67:be3ea5450cc7 37 current_waypoint.theta = (-3.0f/4.0f)*PI;
madcowswe 67:be3ea5450cc7 38 current_waypoint.pos_threshold = 0.03;
rsavitski 54:99d3158c9207 39 current_waypoint.angle_threshold = 0.02*PI;
rsavitski 69:4b7bb92916da 40
madcowswe 66:f1d75e51398d 41 motion::collavoiden = 1;
rsavitski 69:4b7bb92916da 42 motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
rsavitski 55:0c8897da6b3a 43
rsavitski 68:662164480f60 44 float r = 0.61+0.02+0.01;
rsavitski 53:b013df99b747 45
madcowswe 66:f1d75e51398d 46 bool firstavoidstop = 1;
rsavitski 69:4b7bb92916da 47 delayed_struct ds = {Thread::gettid(),&current_waypoint};
rsavitski 70:0da6ca845762 48 RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
rsavitski 76:532d9bc1d2aa 49 RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
rsavitski 69:4b7bb92916da 50
rsavitski 54:99d3158c9207 51 for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
rsavitski 30:791739422122 52 {
rsavitski 39:44d3dea4adcc 53 motion::waypoint_flag_mutex.lock();
rsavitski 69:4b7bb92916da 54 if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop) && delayed_done)
rsavitski 30:791739422122 55 {
rsavitski 65:4709ff6c753c 56 //temphack!!!!!
rsavitski 69:4b7bb92916da 57 //Thread::wait(1000);
rsavitski 65:4709ff6c753c 58 arm::upper_arm.go_down();
rsavitski 69:4b7bb92916da 59 delayed_done = false;
rsavitski 76:532d9bc1d2aa 60 top_arm_up_timer.start(1200);
rsavitski 70:0da6ca845762 61 delayed_wp_set.start(2400);
rsavitski 69:4b7bb92916da 62 //Thread::wait(2000);
rsavitski 69:4b7bb92916da 63 //arm::upper_arm.go_up();
rsavitski 65:4709ff6c753c 64
rsavitski 54:99d3158c9207 65 phi -= 22.5/180*PI;
rsavitski 54:99d3158c9207 66 current_waypoint.x = 1.5-r*cos(phi);
rsavitski 54:99d3158c9207 67 current_waypoint.y = 2-r*sin(phi);
rsavitski 54:99d3158c9207 68 current_waypoint.theta = constrainAngle(phi+PI/2);
rsavitski 56:ed585a82092b 69
rsavitski 56:ed585a82092b 70 //arm offset
rsavitski 56:ed585a82092b 71 current_waypoint.x += 0.0425*cos(current_waypoint.theta);
rsavitski 56:ed585a82092b 72 current_waypoint.y += 0.0425*sin(current_waypoint.theta);
madcowswe 67:be3ea5450cc7 73
madcowswe 67:be3ea5450cc7 74 current_waypoint.pos_threshold = 0.01;
madcowswe 67:be3ea5450cc7 75 current_waypoint.angle_threshold = 0.01*PI;
rsavitski 57:d434ceab6892 76
rsavitski 69:4b7bb92916da 77 //motion::setNewWaypoint(Thread::gettid(),&current_waypoint);
rsavitski 69:4b7bb92916da 78 if (firstavoidstop)
rsavitski 69:4b7bb92916da 79 {
madcowswe 66:f1d75e51398d 80 motion::collavoiden = 0;
madcowswe 66:f1d75e51398d 81 firstavoidstop = 0;
madcowswe 66:f1d75e51398d 82 }
madcowswe 66:f1d75e51398d 83 else
madcowswe 66:f1d75e51398d 84 motion::collavoiden = 1;
rsavitski 30:791739422122 85 }
rsavitski 54:99d3158c9207 86 motion::waypoint_flag_mutex.unlock();
rsavitski 54:99d3158c9207 87
rsavitski 54:99d3158c9207 88 Thread::wait(50);
rsavitski 30:791739422122 89 }
rsavitski 30:791739422122 90 }
rsavitski 30:791739422122 91
rsavitski 76:532d9bc1d2aa 92 void raise_top_arm(const void *dummy)
rsavitski 70:0da6ca845762 93 {
rsavitski 70:0da6ca845762 94 arm::upper_arm.go_up();
rsavitski 70:0da6ca845762 95 }
rsavitski 70:0da6ca845762 96
rsavitski 76:532d9bc1d2aa 97 void raise_bottom_arm(const void *dummy)
rsavitski 76:532d9bc1d2aa 98 {
rsavitski 76:532d9bc1d2aa 99 arm::lower_arm.go_up();
rsavitski 76:532d9bc1d2aa 100 }
rsavitski 76:532d9bc1d2aa 101
rsavitski 69:4b7bb92916da 102 void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
rsavitski 69:4b7bb92916da 103 {
rsavitski 69:4b7bb92916da 104 delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
rsavitski 69:4b7bb92916da 105 motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
rsavitski 69:4b7bb92916da 106 delayed_done = true;
rsavitski 69:4b7bb92916da 107 }
rsavitski 69:4b7bb92916da 108
rsavitski 30:791739422122 109 } //namespace