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 !!*//
- //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=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 = 7;
- const int Blue = 5;
- const int Red = 3;
- int distanzaPrimaFila = 30;
- int tragitto=1;
- int oggettipresi=0;
- /*void moveForward(int v){
- motor[motorA]=v;
- motor[motorB]=v;
- wait1Msec(1);
- }*/
- 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(int 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(int a){
- if(getUSDistance(frontSensor)<=a){
- return true ;
- }
- else return false;
- }
- int color()
- {
- if(getColorName(colorSensor)==colorBlack)
- {
- return 0;
- }
- else if(getColorName(colorSensor)==colorBlue)
- {
- return 1;
- }
- return 2;
- //Funzione che rileva il colore dell' oggetto preso
- }
- 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,-clawLifter_Velocity);
- sleep(500);
- }
- void initialize(){// Funzione di inizializazione
- liftClaw();
- openClaw();
- }
- void liftObject(){ // Funzione prendi oggetto
- movement(0,0);
- fallClaw();
- delay(1000);
- closeClaw();
- delay(1000);
- liftClaw();
- }
- void releaseObject(){//Rilascia oggetto
- movement(0,0);
- fallClaw();
- openClaw();
- liftClaw();
- }
- void turn(char a, int gradi)//Funzione gira di 90°
- {
- resetGyro(gyroscope);
- switch (a)
- {
- case 'L':
- resetGyro(gyroscope);
- while (getGyroDegrees(gyroscope)<gradi)
- {
- motor[motorA]=10;
- motor[motorB]=-10;
- wait1Msec(1);
- }
- break;
- case 'R' :
- resetGyro(gyroscope);
- while (getGyroDegrees(gyroscope)>-gradi )
- {
- motor[motorA]=-10;
- motor[motorB]=10;
- wait1Msec(1);
- }
- break;
- }
- }
- void dropObject_right(int a)//Porta l'ogetto alla cassa
- {
- switch (a)
- {
- case 0:
- movement(-50,-50);
- sleep(1000);
- movement(0,0);
- turn('L',90);
- while(!objectDetectedFront(5))
- {
- movement(50,50);
- }
- movement(0,0);
- openClaw();
- closeClaw();
- turn('R', 180);
- tragitto=1;
- oggettipresi++;
- break;
- case 1:
- movement(-100,-100);
- sleep(1000);
- movement(0,0);
- turn('R',90);
- while(!objectDetectedFront(5))
- {
- movement(50,50);
- }
- movement(0,0);
- openClaw();
- closeClaw();
- tragitto=-1;
- oggettipresi++;
- break;
- }
- }
- void dropObject_left(){
- }
- void routine(int n)// Funzione cerca oggetto è afferra; Warka
- {
- while(!objectDetectedLeft(30)) //Mentre il LeftSensor non rileva oggetti va avanti
- {
- movement(n*moveforwardVelocity,n*moveforwardVelocity);
- }
- distanzaPrimaFila = getUSDistance(leftSensor)+1;
- movement(0,0);//spegnimento motori
- turn('L',90);//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))
- {
- movement(0,0);
- liftObject();
- break;
- }
- }
- }
- void Program1(){
- routine(tragitto);
- movement(0,0);
- dropObject_right(color());
- }
- void Program2()
- {
- }
- task main(){
- initialize();
- while(oggettipresi < 6){Program1();}
- oggettipresi=0;
- while(oggettipresi < 6){Program2();}
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement