void Acquired_Taste(){//dump
motor_off();
shake();
ao();
enable_servos();
servo_set(0, 450,4);//half way up
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);//back up
msleep(2000);
servo_set (0, 20, 2);//full up
ao();
motor(MOT_LEFT, 40);
motor(MOT_RIGHT, 30);//unjam if jammed
msleep(1000);
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);//redump
msleep(1000);
ao();
disable_servos();
msleep(2000);
enable_servos();
motor(MOT_LEFT, 60);
motor(MOT_RIGHT, 30);
servo_set (0, 700, 2);//forward and drop
motor(MOT_LEFT, -40);
motor(MOT_RIGHT, -30);
servo_set (0, 800, 2);
disable_servos();
}
void Luggagecartcafe()
{
enable_servos();
set_servo_position(1,200);
track_update();
int X = 69;
if (track_x(0,0) < track_x(2,0)) //Pink on left
{
track_update();
while (track_x (0,0) > X+5 || track_x (0,0) < X-5)
{
if (track_x(0, 0) > X+5)
{
while(track_x(0,0) > X)
{
track_update();
left(250);
}
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(0, 0) < X)
{
track_update();
right(250);
}
ao();
}
}
}
else if (track_x(2,0) < track_x(0,0)) //Pink on left
{
track_update();
while (track_x (2,0) > X+5 || track_x (2,0) < X-5)
{
if (track_x(2, 0) > X+5)
{
while(track_x(2,0) > X+5)
{
track_update();
left(250);
}
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(2, 0) < X-5)
{
track_update();
right(250);
}
}
}
}
}
void cameratrack()
{
//enable_servos();
//set_servo_position(1,0);
track_update();
if (track_x(0,0) < track_x(2,0)) //Pink on left
{
track_update();
while (track_x (0,0) > 82 || track_x (0,0) <77)
{
if (track_x(0, 0) > 82)
{
while(track_x(0,0) > 82)
{
track_update();
left(700);
}
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(0, 0) < 77)
{
track_update();
right(700);
}
}
}
ao();
movecenterfast(800);
}
else
{
track_update();
while (track_x (2,0) > 82 || track_x (2,0) < 77)
{
if (track_x(2, 0) > 82)
{
while(track_x(2,0) > 82)
{
track_update();
left(100);
}
ao();
}
else // (track_x(0,0) < X)
{
while (track_x(2, 0) < 77)
{
track_update();
right(100);
}
}
}
ao();
movecenterfast(800);
// set_servo_position(1,800);
}
}
void LuggageTrack()
{
enable_servos();
set_servo_position(1,0);
track_update();
int X = 73; //x coord of center of left cart
int y = 5; //how far right/left of center luggage carts can be
if (track_x(0,0) < track_x(2,0)) //Pink on left
{
track_update();
while (track_x (0,0) < X-y || track_x (0,0) > X+y)
{
if (track_x(0, 0) > X+y)
{
while(track_x(0,0) > X+y)
{
track_update();
spinCW(50);
}
ao();
}
else if (track_x(0,0) < X-y)
{
while (track_x(0, 0) < X-y)
{
track_update();
spinCCW(50);
}
}
}
}
else
{
track_update();
while (track_x (2,0) > X+y || track_x (2,0) < X-y)
{
if (track_x(2, 0) > X+y)
{
while(track_x(2,0) > X+y)
{
track_update();
spinCCW(50);
}
ao();
}
else if (track_x(0,0) < X-y)
{
while (track_x(2, 0) < X-y)
{
track_update();
spinCW(50);
}
ao();
}
}
}
}
void mainwindow::learn(int date_open,int date_close){
//нахождение всех необходимы коефициетвов осцилляторов для обучения
qDebug()<<"Training started";
for (int i=date_open;i<=date_close;i++){
mas_ao[i][0]=ao(vector,i);
mas_ao[i][1]=ao(vector,i-7);
mas_ao[i][2]=ao(vector,i-14);
mas_ao[i][3]=ao(vector,i-21);
}
double profit_max=0,w1_max=0,w2_max=0,w3_max=0,w4_max=0;
//обучение(подбор весовых коефициентов, при которых профит максимальный)
bool open=true,close=false;
for (int i1=1;i1<=100;i1++){
QMetaObject::invokeMethod(this, "updateProgress",Q_ARG(int,i1));
for (int i2=1;i2<=100;i2++){
for (int i3=1;i3<=100;i3++){
for (int i4=1;i4<=100;i4++){
//c=0;
float profit=0;
open=true;
close=false;
for (int i=date_open;i<=date_close;i++){
double d=mas_ao[i][0]*(i1-100)+mas_ao[i][1]*(i2-100)+mas_ao[i][2]*(i3-100)+mas_ao[i][3]*(i4-100);
if (open){
if (d>0){
profit=profit-vector[i]->open();
open=false;
close=true;
}
}
if (close){
if (d<0){
profit=profit+vector[i]->close();
close=false;
open=true;
}
}
if ((close)&&(i==date_close)){
profit=profit+vector[i]->close();
close=false;
open=true;
}
if (profit>profit_max){
profit_max=profit;
w1_max=i1;
w2_max=i2;
w3_max=i3;
w4_max=i4;
QMetaObject::invokeMethod(this, "updateUI",Q_ARG(int,w1_max),Q_ARG(int,w2_max),Q_ARG(int,w3_max),Q_ARG(int,w4_max),Q_ARG(double,profit_max));
}
}
}
}
}
}//*/
void findBall(int color)
{
camera_update();
display_clear();
int count = 1;
int objNum = get_object_count(color);
while(objNum == 0)
{
camera_update();
objNum = get_object_count(color);
}
printf("%d time finding a ball", count);
point2 objCen = get_object_center(color, 0);
int errorX = 0, errorY = 0;
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
while(!(a_button_clicked()))
{
if(BALL_NUM_BOX >= 2)
break;
if(errorX > -4 && errorX < 4 && errorY > -4 && errorY < 4)
break;
int turnLM = -1 * errorX * P_X + errorY * P_Y;
int turnRM = errorX * P_X + errorY * P_Y;
if(turnLM > -1 * MINS && turnLM < 0)
turnLM = -1 * MINS;
if(turnLM > 0 && turnLM < MINS)
turnLM = MINS;
if(turnRM > -1 * MINS && turnRM < 0)
turnRM = -1 * MINS;
if(turnRM > 0 && turnRM < MINS)
turnRM = MINS;
motor(LM, turnLM);
motor(RM, turnRM);
camera_update();
int objArea = get_object_area(color, 0);
while(objArea < 200)
{
ao();
camera_update();
objArea = get_object_area(color, 0);
}
count++;
display_clear();
printf("%d time finding a ball ", count);
printf("%d NUM", BALL_NUM_BOX);
objCen = get_object_center(color, 0);
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
}
ao();
catchBalls(color);
}
void biofuelblock() {
turnCCW();
while(digital(12) == 0) {
movecenterfast(900);
}
movecenterbackwards(800);
sleep(032342);
ao();
turnCW();
set_servo_position(1,1900);
while(analog(4) <= 190 && analog(6) <= 190) {
movecenterslow(500);
}
ao();
}
int main()
{
camera_open();
ssp(SORT_SERVO,START);
ssp(RED_SERVO,RED_START);
ssp(GREEN_SERVO,GREEN_START);
wait_for_light(0);
start();
shut_down_in(119);
enable_servos();
msleep(5000);
reset();
right(47,0);
forward(35);
right(10,0);
backward(46);
full_sort();
other_side();
ssp(GREEN_SERVO,GREEN_DUMP);
msleep(1000);
ssp(GREEN_SERVO,GREEN_START);
msleep(250);
now();
ao();
disable_servos();
}
~Sling() {
arm.move_to( iPair(0,0) );
mav(motor_shoulder, -200);
mav(motor_arm, -200);
sleep(0.2);
ao();
}
Message* MessageQueue::obtainMessage(){
Mutex::Autolock ao(mLock);
int tryTimes = 3;
while(!mDone && mFreeList.empty() && tryTimes-- > 0){
mCond.waitRelative(mLock, 100000000);
}
if(mDone)
return NULL;
Message* msg = NULL;
if(!mFreeList.empty()){
msg = *mFreeList.begin();
mFreeList.erase(mFreeList.begin());
mObtainList.push_back(msg);
}else{
msg = (Message*)calloc(sizeof(Message), 1);
mObtainList.push_back(msg);
}
memset(msg, 0, sizeof(Message));
return msg;
}
int avoid_booster()
{
register unsigned int current_ET; // declare ET value
while (1)
{
current_ET = get_ET();
mav(lego.left.port , 100);
mav(lego.right.port , 300);
msleep(10); // arc until you are at the closest point to the wall (should be parallel)
if (current_ET > get_ET())
ao();
mav(lego.left.port , -100);
mav(lego.right.port , -300);
msleep(10);
break;
}
while (1)
{
mav(lego.left.port , 300);
mav(lego.right.port , 300);
msleep(10); // drive to the line
if (get_middle() < THRESH)
break;
}
}
// TEST:
void test(){
ao();
display_clear();
printf("test now");
while(digital(TSS) == 0);
msleep(1000);
}
void LinkSquareUp(int time, float speed) {
mav(RIGHT_PORT, speed*LINK_RIGHT_OFFSET);
mav(LEFT_PORT, speed*LINK_LEFT_OFFSET);
mav(3, 400); //Metal blocker lowered
msleep(time);
ao();
}
inline void get_pom()
{
while (1) // position to get the pom
{
blob_update();
printf("(%d , %d) , (%d , %d)\n" , target.green.x , target.green.y , current.green.x , current.green.y);
if ((current.green.y >= (target.green.y - TOL) && (current.green.y <= target.green.y + TOL)) && (current.green.x >= (target.green.x - TOL) && ( current.green.x <= target.green.x + TOL)))
{
ao();
printf("IN POS\n");
set_servo_position(arm_servo , ARM_DOWN);
msleep(500);
set_servo_position(arm_servo , ARM_OUT);
msleep(500);
set_servo_position(arm_servo , ARM_DOWN);
msleep(500);
set_servo_position(arm_servo , ARM_UP);
msleep(500);
break;
}
camera_move_y();
msleep(10);
camera_move_x();
msleep(10);
}
}
inline int camera_move_y()
{
int speed = 200;
int back = -200;
blob_update();
if (current.green.y > (target.green.y - TOL))
{
printf("TOO CLOSE\n");
mav(lego.left.port , back);
mav(lego.right.port , back);
return 1;
}
if (current.green.y < (target.green.y + TOL))
{
printf("TOO FAR\n");
mav(lego.left.port , speed);
mav(lego.right.port , speed);
return 1;
}
if (current.green.y >= (target.green.y - TOL) && current.green.y <= (target.green.y + TOL))
{
printf("GOLDILOCKS\n");
ao();
return 0;
}
}
// test:
void TEST()
{
ao();
display_clear();
printf("next step ready");
while(!a_button_clicked());
}
void detectlinestop() {
int threshold=512; // set threshold for light conditions
if (analog10(1) >= threshold) { // continue until dark
ao(); // move to line
}
}
bool AudioEngine::setSource(Prefetcher::SubSource* src){
Mutex::Autolock ao(mLock);
mSource = src;
return true;
}
bool AudioEngine::setTimeSync(SyncTimer* timer){
Mutex::Autolock ao(mLock);
mTimer = timer;
return true;
}
void turnCCW() {
mav(0,700);
mav(1,-700);
sleep(0.46);
ao();
}
inline int camera_move_x()
{
int lspeed = -100;
int hspeed = 100;
blob_update();
printf("MOVING X\n");
if (current.green.x < (target.green.x + TOL))
{
printf("LEFT\n");
mav(lego.left.port , lspeed);
mav(lego.right.port , hspeed);
return 1;
}
if (current.green.x > (target.green.x - TOL))
{
printf("RIGHT\n");
mav(lego.left.port , hspeed);
mav(lego.right.port , lspeed);
return 1;
}
if (current.green.x >= (target.green.x - TOL) && current.green.x <= (target.green.x + TOL))
{
ao();
return 0;
}
}
void moveBackwardRoutine(double distanceInInches,int speed, int debug) {
//convert inches to clicks
int clicks =(int) (156.25l * distanceInInches);
int initial_position_right = get_motor_position_counter(RIGHT_MOTOR);
int initial_position_left = get_motor_position_counter(LEFT_MOTOR);
int current_position_right = get_motor_position_counter(RIGHT_MOTOR);
int current_position_left = get_motor_position_counter(LEFT_MOTOR);
int differential = 0 ;
while (current_position_left >= (initial_position_left - clicks) ||
current_position_right >= (initial_position_right - clicks) ) {
//first let's see if one motor is going ahead of the other
differential = current_position_left - initial_position_left -
(current_position_right - initial_position_right);
if (differential > -25 && differential < 25 ) {
mav(RIGHT_MOTOR, speed);
mav(LEFT_MOTOR, speed);
} else if (differential > 0 ) {
mav(RIGHT_MOTOR, (int) (speed*ADJUST_SPEED));
mav(LEFT_MOTOR, (speed * 1.1));
} else {
mav(RIGHT_MOTOR, (speed * 1.1));
mav(LEFT_MOTOR, (int) (speed*ADJUST_SPEED));
}
msleep(25);
current_position_right = get_motor_position_counter(RIGHT_MOTOR);
current_position_left = get_motor_position_counter(LEFT_MOTOR);
}
//turn off motors completely
mav(RIGHT_MOTOR, 0);
mav(LEFT_MOTOR,0);
ao();
reset_motors();
}
void found_something(){
ao();
camera_update();
msleep(100);
camera_update();
msleep(100);
camera_update();
msleep(100);
while(get_object_bbox(0,0).width < 80){
camera_update();
if(get_object_center(0,0).x > 120)
{
motor(Motor_Left,Drivespeed_middle/2);
motor(Motor_Right,-Drivespeed_middle/2);
camera_update();
} else if(get_object_center(0,0).x < 70) {
motor(Motor_Left,-Drivespeed_middle/2);
motor(Motor_Right,Drivespeed_middle/2);
camera_update();
} else {
motor(Motor_Left,Drivespeed_middle/2);
motor(Motor_Right,Drivespeed_middle/2);
camera_update();
}
msleep(5);
}
freeze(Motor_Left);
freeze(Motor_Right);
claw_close();
msleep(1000);
claw_up();
}
/*
* Class: Motor
* Method: ao
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_cbccore_low_Motor_ao(JNIEnv* env, jobject obj)
{
#ifdef CBC
return ao();
#else
printf("Java_cbccore_low_Motor_ao stub\n");
#endif
}
PidTunerWidget::~PidTunerWidget()
{
ao();
#ifndef NOT_A_KOVAN
publish();
#endif
}
CombinedMotorWidget::~CombinedMotorWidget()
{
ao();
#ifdef A_KOVAN
publish();
#endif
delete ui;
}
YumaData::operator AlmOrbit() const
{
AlmOrbit ao(PRN, ecc,i_offset, OMEGAdot, Ahalf, OMEGA0,
w, M0, AF0, AF1, Toa, xmit_time, week, SV_health);
return ao;
} // end of AlmOrbit()
void followLine()//making minor code for following the line
{
printf("1. Checking for touch.\n");
while (digital(10)==0)
{
printf("2. No touch. Checking reflect.\n");
while (analog(4)<=reflectValue)
{
printf("3. Reflect successful. Moving.\n");
motor(1,500);
motor(3,500);
msleep(500);
}
if (analog(4)>reflectValue)
{
printf("4. Reflect unsuccessful. Turning.\n");
motor(1,100);//turn around, make SMALL TURNS, outer pivot
countTime=0; //countTime resets here.
while (countTime<500 && analog(4)>=reflectValue) //countTime variable checks constantly and acts as a delay.
{ //if it either reaches 1000 or detects a line,
countTime=countTime+1; //it moves on and turns.
msleep(2);
}
if (countTime>=500)
{
printf("5. Count time exceeded 1000. Turning.\n");
while (analog(4)>=reflectValue)
{
motor(3,100);
msleep(1000);
ao();
}
}
else
{
printf("6. Reached end of if.\n");
}
} //no "else", because that could give a false positive more easily
printf("7.End of while loop. Deactivating motors...\n");
ao();
}
ao();
}
void MuxEngine::threadEntry(){
for(;;){
Mutex::Autolock ao(mLock);
if(mDone){
break;
}
MediaBuffer* buffer = NULL;
MediaBuffer* out;//TODO: mean nothing.
bool res;
if(mNextReadAudio && mAudioSrc != NULL){
res = mAudioSrc->read(&buffer);
if(!res)
break;
mFFMPEG->encodeAudio(buffer, out);
if(mVideoSrc != NULL)
mNextReadAudio = false;
}else if(!mNextReadAudio && mVideoSrc != NULL){
res = mVideoSrc->read(&buffer);
if(!res)
break;
// mFFMPEG->encodeVideo(buffer, out);
ALOGI("encodeVideo");
if(mAudioSrc != NULL)
mNextReadAudio = true;
}
buffer->release();
}
{
ALOGW("MuxEngine thread exited!!");
Mutex::Autolock ao(mLock);
mThreadExited = true;
mCond.signal();
}
}
bool AudioEngine::resume(){
Mutex::Autolock ao(mLock);
if(!mStarted)
return false;
mMixer->resume();
return true;
}
