Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #pragma config(Sensor, S1, gyroscope, sensorEV3_Gyro)
- #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 !!*//
- //Il nero lo scarta, il rosso lo mette nella chest a DX e il blu a SX.
- const int moveforwardVelocity=20;
- const int claw_Velocity=50;
- const int claw_PositionOpen=3*360;
- const int claw_PositionClose=-2.5*360;
- const int clawLifter_Velocity=-50;
- const int clawLifter_PositionUp=360;
- const int clawLifter_PositionDown=360;
- const double Front_Object_distance = 10;
- const int Left_Object_distance= 38;
- const int Blue = 5;
- const int Red = 3;
- bool compreso= false;
- bool mainore =false;
- bool maggiore =false;
- int tragitto=1;
- int oggettipresi=0;
- int distans=0;
- int contatore = 0;
- void moveForward(double v){
- if(v>0){
- double n= v/20.4
- moveMotorTarget(motorA,n*360, 20);
- moveMotorTarget(motorB,n*360, 20);
- sleep(n*1000);
- }
- else if(v<0){
- double n= v/20.4
- moveMotorTarget(motorA,n*360, -20);
- moveMotorTarget(motorB,n*360, -20);
- sleep(n*1000);
- }
- }
- void movement (int V1, int V2 ){ // Funzione potenza hai motori;
- motor[motorA]=V1;
- motor[motorB]=V2;
- wait1Msec(1);
- }
- //Funzione che rileva la presenza di oggetti sulla sinistra confrontando la distanza con parametro passato alla funzione
- bool objectDetectedLeft(double a){
- if(getUSDistance(leftSensor)<=a){
- return true ;
- }
- else return false;
- }
- //Funzione che rileva la presenza di oggetti di fronte confrontando la distanza con parametro passato alla funzione
- bool objectDetectedFront(double a){
- if(getUSDistance(frontSensor)<=a){
- return true ;
- }
- else return false;
- }
- void openClaw (){ //Funzione apri pinza
- moveMotorTarget(claw,claw_PositionOpen,claw_Velocity);
- sleep(500);
- }
- void closeClaw(){ //Funzione chiudi pinza
- moveMotorTarget(claw,claw_PositionClose,-claw_Velocity);
- sleep(500);
- }
- void liftClaw(){ //Alza braccio
- moveMotorTarget(clawLifter,clawLifter_PositionUp+10,clawLifter_Velocity);
- sleep(500);
- }
- void fallClaw(){ //Abbassa braccio
- moveMotorTarget(clawLifter,clawLifter_PositionDown-50,-clawLifter_Velocity);
- sleep(500);
- }
- void turn(int c, int gradi){
- resetGyro(gyroscope);
- switch(c){
- case 1:
- while(getGyroDegrees(gyroscope)<gradi){
- motor[motorA]=-10;
- motor[motorB]=10;
- wait1Msec(1);}
- break;
- case 2: while(getGyroDegrees(gyroscope)>-gradi){
- motor[motorA]=10;
- motor[motorB]=-10;
- wait1Msec(1);
- }
- break;}
- }
- void initialize(){// Funzione di inizializazione
- liftClaw();
- openClaw();
- moveForward(40);
- turn('R',90);
- moveForward(30);
- }
- void liftObject(){ // Funzione prendi oggetto
- movement(0,0);
- fallClaw();
- delay(1000);
- }
- void releaseObject(){//Rilascia oggetto
- movement(0,0);
- fallClaw();
- openClaw();
- liftClaw();
- }
- task main(){
- //closeClaw();
- liftClaw(); //alza pinza funziona
- openClaw(); // apre pinza funziona
- movement(-30,-30);
- wait1Msec(3000);
- movement(0,0);
- wait1Msec(500);
- movement(10,10);
- wait1Msec(4000);
- movement(0,0);
- wait1Msec(500);
- turn(2,90);
- movement(0,0);
- sleep(500);
- movement(-50,-50);
- sleep(2500);
- movement(0,0);
- sleep(500);
- movement(10,10);
- sleep(5500);
- //AVANTI FIN QUANDO NON RILEVA NULLA A SINISTRA
- while(!objectDetectedLeft(Left_Object_distance)){
- movement(20,20);
- sleep(500);
- movement(0,0);
- sleep(500);
- }
- // UNA VOLTA RILEVATO GIRA A SINISTRA
- moveForward(-6);
- sleep(1000);
- while(!objectDetectedFront(20)){movement(-10,10);delay(100);}
- while(!objectDetectedFront(Front_Object_distance)){
- movement(10,10);
- sleep(500);
- movement(0,0);
- sleep(500);
- }
- //PRENDE L'OGGETTO
- liftObject();
- //giusto
- if(getColorName(colorSensor)!= colorBlue && getColorName(colorSensor) != colorRed){
- movement(10,10);
- sleep(350);
- movement(0,0);
- sleep(500);
- closeClaw();
- sleep(1000);
- liftclaw();
- sleep (2000);
- movement(-20,-20);
- delay(4000);
- turn(2,90);
- movement(0,0);
- delay(500);
- while(!objectDetectedFront(5))
- {
- movement(50,50);
- }
- openClaw();
- sleep(500);
- closeClaw();
- movement(-10,10);
- sleep(1000);
- }
- //funziona
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement