Stap 6: ToBe automatisering - kleur sorter robot - Artibox programmering
Programma voor het Artibox-board
Headerbestand te houden van de poses
//// ToBeAut_Robotics.h // // // Created by Tomas Rus on 01/06/16. // //#ifndef ToBeAut_Robotics_h #define ToBeAut_Robotics_h#include //hardcoded main poses PROGMEM prog_uint16_t homepose[] = {5, 492, 185, 893, 757, 512}; //home PROGMEM prog_uint16_t centered[] = {5, 213, 512, 512, 512, 256}; //centered PROGMEM prog_uint16_t abovecon[] = {5, 690, 530, 744, 715, 512};//above conveyor PROGMEM prog_uint16_t xabovecon[] = {5, 690, 530, 744, 715, 200}; //holded PROGMEM prog_uint16_t undercam[] = {5, 175, 310, 971, 711, 200};// in front of the cam PROGMEM prog_uint16_t pile1[] = {5, 343, 543, 866, 729, 200};//pile 1 PROGMEM prog_uint16_t xpile1[] = {5, 343, 543, 866, 729, 512};//pile 1 gripper open PROGMEM prog_uint16_t pile2[] = {5, 422, 724, 644, 599, 200}; //pile 2 PROGMEM prog_uint16_t xpile2[] = {5, 422, 724, 644, 599, 512}; //pile 2 gripper open PROGMEM prog_uint16_t dump[] = {5, 353, 378, 268, 328, 200}; //dump PROGMEM prog_uint16_t xdump[] = {5, 353, 378, 268, 328, 512}; //dump gripper open#endif /* ToBeAut_Robotics_h */ Werkelijke arduino programma
//// ToBeAut_Robotics.cpp // // // Created by Tomas Rus on 01/06/16. // // // Main program for ArbotiX PhantomX Pincher // Class - aut/0915 // Author: Tomas Rus, Benedikt Zaleha, Christian Hejlesen#include "ToBeEssentials.h" //hardcoded Poses #include //Pixy libraries #include // #include //library for AX12 servos #include //library for Move function and many more #include //small library for led operation, can be replacedBioloidController bioloid = BioloidController(1000000);//Global variables go here LED13 ledgreen; LED13 ledred; int PshBut = 0; //push button 1 pin number int val = 0; int fromPLC = 3; //pin for input from PLC, if there'ß a box int readySeq = 18; //pin for sending ready signal via relay to the PLC //recognizing the colours, colour 1 = blue, colour = 2 = green, else 0 int moveDelay = 1000; //speed of the robot in ms int Pressure; int colour = 0; int boxW = 0; //box waiting on the conveyor, internal bitvoid setup (){ pinMode(3, INPUT); pinMode(16, OUTPUT); pinMode(17, OUTPUT); pinMode(18, OUTPUT); }void loop () { delay(1000); Move(homepose); ledred.off(17); ledgreen.on(16); //signalizing ready status with green led boxW = 0; boxW = digitalRead(fromPLC); delay(1000); if (boxW) { ledgreen.off(16); //signalizing operation status with red led ledred.on(17); playSequence(); } } // void loop end void Move(prog_uint16_t* Inn) //inherited from bioloid library { delay(moveDelay); // recommended pause bioloid.loadPose(Inn); // load the pose from FLASH, into the nextPose buffer bioloid.readPose(); // read in current servo positions to the curPose buffer delay(moveDelay); bioloid.interpolateSetup(moveDelay); // setup for interpolation from current->next over 1/2 a second while(bioloid.interpolating > 0) { // do this while we have not reached our new pose bioloid.interpolateStep(); // move servos, if necessary. delay(3); } } void playSequence() { delay(1000); Move(abovecon); delay(1000); Move(xabovecon); /* while (Pressure<500) { //optional use of pressure sensor Pressure = analogRead(26); delay(200); gripper -= 30; if (gripper <0) { gripper = 0; } ax12SetRegister2(5, 30, gripper); } */ delay(1000); Move(undercam); delay(1000); colour = getColor(); switch (colour) { case '1': //moving boxes to the first pile Move(pile1); delay(1000); Move(xpile1); delay(1000); break; case '2': //moving boxes to he second pile Move(pile2); delay(1000); Move(xpile2); delay(1000); break; default: //moving unrecognized boxes to the dump pile Move(dump); delay(1000); Move(xdump); break; } Move(homepose); }int getColor() { Pixy pixy; pixy.init(); delay(500); int blocks; int j; int i = 0; int color = 0; while (color != 1 || color != 2) { blocks = pixy.getBlocks(); if (blocks) { i++; // do this (print) every 20 frames because printing every // frame would bog down the Arduino if (i%20==0) { Serial.println(color); color = pixy.blocks[j].signature; return color; } } } }
