Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #pragma config(Sensor, S1, gyroscope, sensorEV3_Gyro, modeEV3Gyro_RateAndAngle)
- #pragma config(Sensor, S2, colorSensor, sensorEV3_Color, modeEV3Color_Color)
- #pragma config(Sensor, S3, frontSensor, sensorEV3_Ultrasonic)
- #pragma config(Sensor, S4, leftSensor, sensorEV3_Ultrasonic)
- #pragma config(Motor, motorA, leftMotor, tmotorEV3_Large, PIDControl, driveLeft, encoder)
- #pragma config(Motor, motorB, rightMotor, tmotorEV3_Large, PIDControl, driveRight, encoder)
- #pragma config(Motor, motorC, clawLifter, tmotorEV3_Large, PIDControl, encoder)
- #pragma config(Motor, motorD, claw, tmotorEV3_Medium, PIDControl, encoder)
- //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
- const int moveforwardVelocity=20;
- const int claw_Velocity=100;
- const int claw_PositionOpen=3*360;
- const int claw_PositionClose=-3*360;
- const int clawLifter_Velocity=-50;
- const int clawLifter_PositionUp=360;
- const int clawLifter_PositionDown=360;
- const int Object_distance = 5;
- const int Blue = 5;
- const int Red = 3;
- void moveForward(int v){
- motor[motorA]=v;
- motor[motorB]=v;
- wait1Msec(1);
- }
- void movement (int V1, int V2 ){
- //setMotorSpeed(motorA, V1);
- //setMotorSpeed(motorB, V2);
- motor[motorA]=V1;
- motor[motorB]=V2;
- wait1Msec(1);
- }
- bool objectDetectedLeft(int a)
- {
- if(getUSDistance(leftSensor)<=a)
- {
- return true;
- }
- else return false;
- }
- bool objectDetectedFront(int a){
- if(getUSDistance(frontSensor)<=a){
- return true ;
- }
- else return false;
- }
- void openClaw (){
- moveMotorTarget(claw,claw_PositionOpen,claw_Velocity);
- sleep(500);
- }
- void closeClaw(){
- moveMotorTarget(claw,claw_PositionClose,-claw_Velocity);
- sleep(500);
- }
- void liftClaw(){
- moveMotorTarget(clawLifter,clawLifter_PositionUp+10,clawLifter_Velocity);
- sleep(500);
- }
- void fallClaw(){
- moveMotorTarget(clawLifter,clawLifter_PositionDown,-clawLifter_Velocity);
- sleep(500);
- }
- void inizialize(){
- liftClaw();
- openClaw();
- }
- void liftObject(){
- movement(0,0);
- fallClaw();
- delay(1000);
- closeClaw();
- delay(1000);
- liftClaw();
- }
- void releaseObject(){
- movement(0,0);
- fallClaw();
- openClaw();
- liftClaw();
- }
- void turn( char a)
- {
- resetGyro(gyroscope);
- switch (a)
- {
- case 'L':
- resetGyro(gyroscope);
- while (getGyroDegrees(gyroscope)<89)
- {
- motor[motorA]=10;
- motor[motorB]=-10;
- wait1Msec(1);
- }
- break;
- case 'R' :
- resetGyro(gyroscope);
- while (getGyroDegrees(gyroscope)>-89 )
- {
- motor[motorA]=-10;
- motor[motorB]=10;
- wait1Msec(1);
- }
- break;
- }
- }
- void dropObject()
- {
- movement(-10,-10);
- sleep(1000);
- movement(0,0);
- turn('R');
- while(!objectDetectedFront(3))
- {
- moveForward(10);
- }
- movement(0,0);
- openClaw();
- }
- void routine()// Funzione cerca oggetto è afferra; Warka
- {
- while(!objectDetectedLeft(20)) //Mentre il LeftSensor non rileva oggetti va avanti
- {
- movement(moveforwardVelocity,moveforwardVelocity);
- }
- movement(0,0);//spegnimento motori
- turn('L');//gira a sinistra di 90°
- movement(0,0);//spegnimento motori
- while(!objectDetectedFront(Object_distance))//Mentre la distanza è maggiore di Object_distance va avanti
- {
- movement(10,10);
- if(objectDetectedFront(Object_distance+0.5))
- {
- movement(0,0);
- liftObject();
- break;
- }
- }
- }
- void Program(){
- inizialize();
- routine();
- movement(0,0);
- dropObject();
- }
- task main(){
- Program();
- //closeClaw();
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement