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 = 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 ){ // Funzione potenza hai motori;
- motor[motorA]=V1;
- motor[motorB]=V2;
- wait1Msec(1);
- }
- bool objectDetectedLeft(int a)//Funzione che rileva la presenza di oggetti sulla sinistra confrontando la distanza con parametro passato alla funzione
- {
- if(getUSDistance(leftSensor)<=a)
- {
- return true;
- }
- else return false;
- }
- bool objectDetectedFront(int a){//Funzione che rileva la presenza di oggetti di fronte confrontando la distanza con parametro passato alla funzione
- if(getUSDistance(frontSensor)<=a){
- return true ;
- }
- else return false;
- }
- int color()
- {
- if(getColorName(colorSensor)==colorBlack)
- {
- return 0;
- }
- else if(getColorName(colorSensor)==colorBlue)
- {
- return 1;
- }
- else if(getColorName(colorSensor)==colorRed)
- {
- return 2;
- }
- //Funzione che rileva il colore del''ogetto 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 inizialize(){// 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)//Funzione gira di 90°
- {
- resetGyro(gyroscope);
- switch (a)
- {
- case 'L':
- resetGyro(gyroscope);
- while (getGyroDegrees(gyroscope)<99)
- {
- motor[motorA]=10;
- motor[motorB]=-10;
- wait1Msec(1);
- }
- break;
- case 'R' :
- resetGyro(gyroscope);
- while (getGyroDegrees(gyroscope)>-99 )
- {
- motor[motorA]=-10;
- motor[motorB]=10;
- wait1Msec(1);
- }
- break;
- }
- }
- void dropObject(int a)//Porta l'ogetto alla cassa
- {
- switch (a)
- {
- case 0:break;
- case 1:break;
- case 2:break;
- }
- /* movement(-10,-10);
- sleep(1000);
- movement(0,0);
- turn('R');
- movement(0,0);
- delay(500);
- while(!objectDetectedFront(3.5))
- {
- movement(10,10);
- }
- movement(0,0);
- openClaw();*/
- }
- void routine(int n)// Funzione cerca oggetto è afferra; Warka
- {
- while(!objectDetectedLeft(20)) //Mentre il LeftSensor non rileva oggetti va avanti
- {
- movement(n*moveforwardVelocity,n*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))
- {
- movement(0,0);
- liftObject();
- break;
- }
- }
- }
- void Program(){
- inizialize();
- routine(1);
- movement(0,0);
- dropObject(color());
- delay(500);
- closeClaw();
- delay(500);
- openClaw();
- delay(500);
- routine(-1);
- }
- task main(){
- //resetGyro(gyroscope);
- //turn('L');
- Program();
- //closeClaw();
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement