zhaw_st16b_pes2_10
k
Dependencies: Servo ServoArm mbed
Diff: main.cpp
- Revision:
- 0:15a8480061e8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,341 @@
+#include "mbed.h"
+#include "Robot.h"
+#include "Declarations.h"
+#include "Ultraschall.h"
+#include "Pixy.h"
+#include "PID_Control.h"
+#include "LowpassFilter.h"
+#include "EncoderCounter.h"
+
+#include <cstdlib>
+
+//Cam:
+Serial cam(PA_9, PA_10);
+Pixy pixy(cam);
+
+//DistanceSensors related bottom:
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+AnalogIn sensorVoltage(PB_1);
+DigitalOut enable(PC_1);
+DigitalOut bit0(PH_1);
+DigitalOut bit1(PC_2);
+DigitalOut bit2(PC_3);
+
+//DistanceSensors top:
+AnalogIn frontS(A1);
+AnalogIn leftS(A2);
+AnalogIn rightS(A3);
+
+
+//Ultraschall
+Ultraschall usensor(D6,D5);
+
+//Leds related:
+DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
+
+//motor related:
+PwmOut left(PA_8);
+PwmOut right(PA_9);
+
+DigitalOut powerSignal(PB_2);
+DigitalIn errorMotor(PB_14);
+DigitalIn overtemperatur(PB_15);
+
+//Arm:
+ServoArm servoArm(PA_6);
+
+//Greifer:
+Servo servoGreifer(PC_7);
+
+//Leiste:
+Servo servoLeiste(PB_6);
+
+//Button:
+DigitalIn mybutton(USER_BUTTON);
+
+//Farbsensor:
+AnalogIn FarbVoltage(A0);
+//DigitalOut led(D2);
+
+
+Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor, &pixy ); //Implement the Farbsensor into the Robot init function!!
+
+void initializeDistanceSensors()
+{
+ for( int ii = 0; ii<9; ++ii) {
+ sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
+
+ enable = 1;
+ }
+}
+/* */
+int main(){
+ initializeDistanceSensors(); //Initialises IR-Sensors.
+ enum states {search = 0, setX, setY, fine, take, test};
+ int time = 0; //Time keeps track of time. [time] = ms
+
+ PID_Control pid;
+ pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
+ pc.baud( 115200 );
+
+ int state = search;
+
+ sam.stop();
+
+ /*
+ while( 1 ){
+ printf("\n\rX: %d,\t Y: %d", pixy.getX(), pixy.getY());
+ wait(1.0f);
+ }
+ */
+
+ while( 1 ){
+ switch( state ){
+ case test:
+ int x = pixy.getX(), y = pixy.getY();
+ if( !mybutton ) printf("\n\rX:%d\tY:%d", x, y);
+ break;
+
+ case search:
+ if( sam.search(&time) ) state = found;
+ break;
+
+ case setX:{
+ static int messungen[20], i = 0;
+ float e = 132.5f - pixy.getX();
+ float diff = pid.calc( e, 0.001f );
+
+ sam.setLeft(0.5f - diff);
+ sam.setRight(0.5f - diff);
+ break;
+ }
+ case setY:{
+ static int messungen[20], i = 0;
+ float e = 121.5f - pixy.getY();
+ float diff = pid.calc( e, 0.001f );
+
+ sam.setLeft(0.5f + diff);
+ sam.setRight(0.5f - diff);
+ break;
+ }
+
+ case take:{
+ //sam.leds[1] = 1;
+ sam.stop();
+ while (1) wait(0.1f);
+ static int down = 0, closed = 0, up = 0;
+ if( down || sam.Arm.backToDown() ) down = 1;
+ else if( closed || sam.Greifer.take() ) closed = 1;
+ else if( up || sam.Arm.downToBack() ) up = 1;
+ else if( sam.Greifer.leave() ){
+ state = search;
+ down = 0;
+ closed = 0;
+ up = 0;
+ }
+ break;
+ }
+ }
+ time++;
+ wait(0.001f);
+ }
+
+ return 0;
+}
+
+/* * /
+int main()
+{
+ initializeDistanceSensors(); //Initialises IR-Sensors.
+ //int counter = 0; //Counts how many times the robot has turned, before driving
+ int timer = 0; //Is used, to reset the robot. Returns time in [0.1s]
+ //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
+ //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
+ //int done = 0;
+ int color;
+
+ enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
+
+ int state = search;
+
+ static float messung = 0;
+
+ while( 1 ){
+ printf("\n\r%f", sam.see(FWD_L));
+
+ wait(1.0f);
+
+ }
+
+ while( 1 ) {
+
+ if ( timer > TIMEOUT ) {
+ NVIC_SystemReset(); //Resets Sam.
+ }
+
+ //if( timer == 0 )
+ //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
+
+
+ //printf("\n\rcurrent main state: %d", state);
+
+ sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
+ switch( state ) {
+ case search:
+ if( sam.search(&timer) ){
+ //sam.Greifer.nullPos();
+ state = LeisteDown;
+ timer = 0;
+ }
+ break;
+
+ case LeisteDown:
+ int count = 0;
+ if( sam.Leiste.upToDown() ){
+ //sam.Greifer.leave();
+ state = turn;
+ timer = 0;
+ }
+ break;
+
+ case turn:
+ static int i = 0;
+ if( i > 7 ){
+ sam.stop();
+ state = push;
+ i = 0;
+ }
+ else{
+ i++;
+ sam.turnRight();
+ }
+ break;
+
+ case push:{
+ static int i = 0;
+ if( i > 5 ){
+ sam.stop();
+ i = 0;
+ state = backOff;
+ timer = 0;
+ }
+ else{
+ sam.driveSlowly();
+ i++;
+ }
+ break;
+ }
+
+ case backOff:{
+ static int i = 0;
+ if( i > 1 ){
+ sam.stop();
+ i = 0;
+ state = forward;
+ timer = 0;
+ }
+ else{
+ sam.driveBackSlowly();
+ i++;
+ }
+ break;
+ }
+
+
+ case forward:
+ if( sam.Arm.backToCollect() ){
+ state = downward;
+ timer = 0;
+ }
+ break;
+
+ case downward:
+ if( sam.Arm.collectToDown() ){
+ state = down;
+ timer = 0;
+ }
+ break;
+
+ case down:
+ if( sam.Greifer.take() ) {
+ state = upward;
+ timer = 0;
+ }
+ break;
+
+ case upward:
+ if( sam.Arm.downToCollect() ){
+ state = colorS;
+ timer = 0;
+ }
+ break;
+
+ case colorS:
+ led = 1;
+ color = sam.FarbVoltage.read();
+
+ if( color == -1 ){
+ //Do nothing
+ }
+
+
+ else if( color == 0 || color == GREEN ){
+ state = backward;
+ led = 0;
+ timer = 0;
+ }
+
+ else if( color == RED ){
+ state = readyDrop;
+ led = 0;
+ timer = 0;
+ }
+
+ else{
+ //Shit...
+ }
+ break;
+
+ case readyDrop:
+ if( sam.Greifer.leave() ){
+ if( color == GREEN || color == 0 ){
+ state = LeisteUp;
+ }
+ else{
+ state = backward;
+ }
+ timer = 0;
+ }
+
+ break;
+
+ case backward:
+ if( sam.Arm.collectToBack() ){
+ //sam.Greifer.nullPos();
+ state = LeisteUp;
+ timer = 0;
+ if( color == GREEN || color == 0 ){
+ state = readyDrop;
+ sam.Greifer.leave();
+ }
+ else{
+ state = LeisteUp;
+ }
+ }
+ break;
+
+ case LeisteUp:
+ if( sam.Leiste.downToUp() ){
+ state = search;
+ timer = 0;
+ }
+ break;
+ }
+ timer++;
+ wait(0.1f);
+ }
+
+ return 0;
+}
+/* */
\ No newline at end of file