Revision c76baf23

View differences:

devices/DiWheelDrive/DiWheelDrive.cpp
97 97 
        return RDY_OK;
98 98 
      }
99 99 
      break;
100 

  		




  
  100
  
    
    case CAN::SET_LINE_FOLLOW_SPEED:


  		




  
  101
  
    
	  if (frame->DLC == 8) {


  		




  
  102
  
    
		  uint8_t speedForward    = frame->data8[0];


  		




  
  103
  
    
		  uint8_t speedSoftLeft0  = frame->data8[1];


  		




  
  104
  
    
		  uint8_t speedSoftLeft1  = frame->data8[2];


  		




  
  105
  
    
		  uint8_t speedHardLeft0  = frame->data8[3];


  		




  
  106
  
    
		  uint8_t speedHardLeft1  = frame->data8[4];


  		




  
  107
  
    
		  global.rpmForward[0] = speedForward;


  		




  
  108
  
    
		  global.rpmForward[1] = speedForward;


  		




  
  109
  
    
		  global.rpmSoftLeft[0] = speedSoftLeft0;


  		




  
  110
  
    
		  global.rpmSoftLeft[1] = speedSoftLeft1;


  		




  
  111
  
    
		  global.rpmHardLeft[0] = speedHardLeft0;


  		




  
  112
  
    
		  global.rpmHardLeft[1] = speedHardLeft1;


  		




  
  113
  
    
		  global.rpmSoftRight[0] = global.rpmSoftLeft[1];


  		




  
  114
  
    
		  global.rpmSoftRight[1] = global.rpmSoftLeft[0];


  		




  
  115
  
    
		  global.rpmHardRight[0] = global.rpmHardLeft[1];


  		




  
  116
  
    
		  global.rpmHardRight[1] = global.rpmHardLeft[0];


  		




  
  117
  
    
		  return RDY_OK;


  		




  
  118
  
    
	  }


  		




  101
  119
  
    
    case CAN::SET_KINEMATIC_CONST_ID:


  		




  102
  120
  
    
      if (frame->DLC == 8) {


  		




  103
  121
  
    
/*        // Set (but do not store) Ed


  		












  

    
      devices/DiWheelDrive/Makefile
    
  





  123
  123
  
    
         $(AMIRO)/components/serial_reset/serial_can_mux.cpp \


  		




  124
  124
  
    
         DiWheelDrive.cpp \


  		




  125
  125
  
    
         userthread.cpp \


  		




  
  126
  
    
         linefollow2.cpp \


  		




  126
  127
  
    
         main.cpp \


  		




  127
  128
  
    
         exti.cpp


  		




  128
  129
  
    

  		












  

    
      devices/DiWheelDrive/global.hpp
    
  





  164
  164
  
    
  DiWheelDrive robot;


  		




  165
  165
  
    

  		




  166
  166
  
    
  UserThread userThread;


  		




  
  167
  
    
  int rpmForward[2] = {25,25};


  		




  
  168
  
    
  int rpmSoftLeft[2] = {15,25};


  		




  
  169
  
    
  int rpmHardLeft[2] = {10,25};


  		




  
  170
  
    
  int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};


  		




  
  171
  
    
  int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};


  		




  167
  172
  
    

  		




  168
  173
  
    
public:


  		




  169
  174
  
    
  Global() :


  		












  

    
      devices/DiWheelDrive/linefollow2.cpp
    
  





  
  1
  
    
#include "global.hpp"


  		




  
  2
  
    
#include "linefollow2.hpp" 


  		




  
  3
  
    

  		




  
  4
  
    

  		




  
  5
  
    
void LineFollow::printSensorData(){


  		




  
  6
  
    
    chprintf((BaseSequentialStream*) &SD1, "Test!");


  		




  
  7
  
    
}


  		




  
  8
  
    

  		




  
  9
  
    
void LineFollow::followLine(int vcnl4020Proximity[4], Global *global){


  		




  
  10
  
    
    chprintf((BaseSequentialStream*) &SD1, "Proximity: WL:0x%04X FL:0x%04X FR:0x%04X WR:0x%04X\n",


  		




  
  11
  
    
                    vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],


  		




  
  12
  
    
                    vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],


  		




  
  13
  
    
                    vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],


  		




  
  14
  
    
                    vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);


  		




  
  15
  
    
    global->motorcontrol.printGains();


  		




  
  16
  
    
}


  		












  

    
      devices/DiWheelDrive/linefollow2.hpp
    
  





  
  1
  
    
#ifndef AMIRO_LINEFOLLOWING_H


  		




  
  2
  
    
#define AMIRO_LINEFOLLOWING_H


  		




  
  3
  
    

  		




  
  4
  
    
#include <ch.hpp>


  		




  
  5
  
    

  		




  
  6
  
    
#include <amiroosconf.h>


  		




  
  7
  
    

  		




  
  8
  
    
namespace amiro {


  		




  
  9
  
    

  		




  
  10
  
    
class LineFollow


  		




  
  11
  
    
{


  		




  
  12
  
    
public:


  		




  
  13
  
    
  void printSensorData();


  		




  
  14
  
    
  void followLine(int vcnl4020Proximity[4], Global *global);


  		




  
  15
  
    
};


  		




  
  16
  
    

  		




  
  17
  
    
} // end of namespace amiro


  		




  
  18
  
    

  		




  
  19
  
    
#endif // AMIRO_LINEFOLLOWING_H


  		












  

    
      devices/DiWheelDrive/userthread.cpp
    
  





  2
  2
  
    

  		




  3
  3
  
    
#include "global.hpp"


  		




  4
  4
  
    

  		




  
  5
  
    
#include "linefollow2.hpp" 


  		




  
  6
  
    

  		




  5
  7
  
    
using namespace amiro;


  		




  6
  8
  
    

  		




  7
  9
  
    
extern Global global;


  		




  ......




  55
  57
  
    
int policyCounter = 0; // Do not change this, it points to the beginning of the policy


  		




  56
  58
  
    

  		




  57
  59
  
    
// Different speed settings (all values in "rounds per minute")


  		




  58
  
  
    
const int rpmForward[2] = {25,25};


  		




  59
  
  
    
const int rpmSoftLeft[2] = {15,25};


  		




  60
  
  
    
const int rpmHardLeft[2] = {10,25};


  		




  61
  
  
    
const int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};


  		




  62
  
  
    
const int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};


  		




  63
  60
  
    
const int rpmTurnLeft[2] = {-10, 10};


  		




  64
  61
  
    
const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};


  		




  65
  62
  
    
const int rpmHalt[2] = {0, 0};


  		




  ......




  111
  108
  
    
void copyRpmSpeed(const int (&source)[2], int (&target)[2]) {


  		




  112
  109
  
    
	target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL];


  		




  113
  110
  
    
	target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL];


  		




  
  111
  
    
	chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);


  		




  114
  112
  
    
}


  		




  115
  113
  
    

  		




  116
  114
  
    
// Fuzzyfication of the sensor values


  		




  ......




  198
  196
  
    
	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&


  		




  199
  197
  
    
	    member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {


  		




  200
  198
  
    
		// straight


  		




  201
  
  
    
		copyRpmSpeed(rpmForward, rpmFuzzyCtrl);


  		




  
  199
  
    
		copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl);


  		




  202
  200
  
    
	// exact one front sensor detects a line


  		




  203
  201
  
    
	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK ||


  		




  204
  202
  
    
	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {


  		




  205
  203
  
    
		// soft correction


  		




  206
  204
  
    
		if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {


  		




  207
  205
  
    
			// soft right


  		




  208
  
  
    
			copyRpmSpeed(rpmSoftRight, rpmFuzzyCtrl);


  		




  
  206
  
    
			copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl);


  		




  209
  207
  
    
		} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {


  		




  210
  208
  
    
			// hard right


  		




  211
  
  
    
			copyRpmSpeed(rpmHardRight, rpmFuzzyCtrl);


  		




  
  209
  
    
			copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);


  		




  212
  210
  
    
		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {


  		




  213
  211
  
    
			// soft left


  		




  214
  
  
    
			copyRpmSpeed(rpmSoftLeft, rpmFuzzyCtrl);


  		




  
  212
  
    
			copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl);


  		




  215
  213
  
    
		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {


  		




  216
  214
  
    
			// hard left


  		




  217
  
  
    
			copyRpmSpeed(rpmHardLeft, rpmFuzzyCtrl);


  		




  
  215
  
    
			copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl);


  		




  218
  216
  
    
		}


  		




  219
  217
  
    
	// both wheel sensors detect a line


  		




  220
  218
  
    
	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK &&


  		




  ......




  288
  286
  
    
	}


  		




  289
  287
  
    
}


  		




  290
  288
  
    

  		




  
  289
  
    
//void lineFollowing_new(xyz) {}


  		




  
  290
  
    

  		




  
  291
  
    
void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]){


  		




  
  292
  
    
	// all sensors are equal


  		




  
  293
  
    
	// if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&


  		




  
  294
  
    
	//     member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&


  		




  
  295
  
    
	//     member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {


  		




  
  296
  
    
	// 	// something is wrong -> stop


  		




  
  297
  
    
	// 	copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);


  		




  
  298
  
    
	// // both front sensor detect a line


  		




  
  299
  
    
	if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&


  		




  
  300
  
    
	    (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY)) {


  		




  
  301
  
    
		// straight


  		




  
  302
  
    
		copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl);


  		




  
  303
  
    
	// Deviation to right


  		




  
  304
  
    
	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK


  		




  
  305
  
    
		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){


  		




  
  306
  
    
			copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl);


  		




  
  307
  
    
	// Deviation to left


  		




  
  308
  
    
	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK


  		




  
  309
  
    
		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){


  		




  
  310
  
    
			copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl);


  		




  
  311
  
    
	// Hard deviatio to right


  		




  
  312
  
    
	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY


  		




  
  313
  
    
		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){


  		




  
  314
  
    
			copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl);


  		




  
  315
  
    
	// Hard deviation to left


  		




  
  316
  
    
	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY


  		




  
  317
  
    
		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){


  		




  
  318
  
    
			copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);


  		




  
  319
  
    
	// Turn if white


  		




  
  320
  
    
	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE


  		




  
  321
  
    
		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE ){


  		




  
  322
  
    
			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);


  		




  
  323
  
    
	}


  		




  
  324
  
    
}


  		




  
  325
  
    

  		




  291
  326
  
    
// Line following by a fuzzy controler


  		




  292
  327
  
    
void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {


  		




  293
  328
  
    
	// FUZZYFICATION


  		




  ......




  312
  347
  
    
	global.robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT]));


  		




  313
  348
  
    
	global.robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]));


  		




  314
  349
  
    

  		




  315
  
  
    
//	chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftFuzzyMemberValues[BLACK], leftFuzzyMemberValues[GREY], leftFuzzyMemberValues[WHITE]);


  		




  316
  
  
    
//	chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);


  		




  
  350
  
    
	// chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftWheelFuzzyMemberValues[BLACK], leftWheelFuzzyMemberValues[GREY], leftWheelFuzzyMemberValues[WHITE]);


  		




  
  351
  
    
	// chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);


  		




  317
  352
  
    

  		




  318
  353
  
    
	// DEFUZZYFICATION


  		




  319
  
  
    
	defuzzyfication(member, rpmFuzzyCtrl);


  		




  
  354
  
    
	// defuzzyfication(member, rpmFuzzyCtrl);


  		




  
  355
  
    
	defuzz(member, rpmFuzzyCtrl);


  		




  320
  356
  
    
}


  		




  321
  357
  
    

  		




  
  358
  
    

  		




  
  359
  
    

  		




  
  360
  
    

  		




  322
  361
  
    
// Set the speed by the array


  		




  323
  362
  
    
void setRpmSpeed(const int (&rpmSpeed)[2]) {


  		




  324
  363
  
    
	global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);


  		




  ......




  355
  394
  
    
		global.robot.setLightColor(led, Color(Color::BLACK));


  		




  356
  395
  
    
    }


  		




  357
  396
  
    
    running = false;


  		




  358
  
  
    

  		




  
  397
  
    
	LineFollow lf;


  		




  359
  398
  
    
	/*


  		




  360
  399
  
    
	 * LOOP


  		




  361
  400
  
    
	 */


  		




  ......




  369
  408
  
    
        /*


  		




  370
  409
  
    
         * evaluate the accelerometer


  		




  371
  410
  
    
         */


  		




  372
  
  
    
        if (accel_z < -900 /*-0.9g*/) {


  		




  
  411
  
    

  		




  
  412
  
    
	//Features over can: (Line Following)


  		




  
  413
  
    
	//SetKP


  		




  
  414
  
    
	//SetKI


  		




  
  415
  
    
	//SetKd ?


  		




  
  416
  
    
	//SetSleepTime


  		




  
  417
  
    
	//SeForwardSpeed? or SetMaxSpeed


  		




  
  418
  
    
	//DriveOnLeftLine


  		




  
  419
  
    
	//DriveOnRightLine


  		




  
  420
  
    
	


  		




  
  421
  
    
	//if accel_z<-900


  		




  
  422
  
    
		//send can event (one time)


  		




  
  423
  
    
		// Line following is started if amiro roteted


  		




  
  424
  
    
        if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature


  		




  373
  425
  
    
            if (running) {


  		




  374
  426
  
    
                // stop the robot


  		




  375
  427
  
    
                running = false;


  		




  ......




  400
  452
  
    
                vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();


  		




  401
  453
  
    
                vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();


  		




  402
  454
  
    
            }


  		




  403
  
  
    

  		




  404
  
  
    
//            chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",


  		




  405
  
  
    
//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],


  		




  406
  
  
    
//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],


  		




  407
  
  
    
//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],


  		




  408
  
  
    
//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);


  		




  409
  
  
    

  		




  
  455
  
    
			lf.followLine(vcnl4020Proximity, &global);


  		




  
  456
  
    
            // chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",


  		




  
  457
  
    
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],


  		




  
  458
  
    
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],


  		




  
  459
  
    
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],


  		




  
  460
  
    
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);


  		




  
  461
  
    
	//if (configLineFollowing==2)


  		




  
  462
  
    
	//   lineFollownew


  		




  
  463
  
    
	//else


  		




  410
  464
  
    
            lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl);


  		




  411
  465
  
    
            setRpmSpeed(rpmFuzzyCtrl);


  		




  412
  466
  
    
        }


  		












  

    
      devices/LightRing/userthread.cpp
    
  





  24
  24
  
    
     * INSERT CUSTOM CODE HERE


  		




  25
  25
  
    
     */


  		




  26
  26
  
    

  		




  
  27
  
    
     


  		




  
  28
  
    

  		




  27
  29
  
    
    this->sleep(MS2ST(1000));


  		




  28
  30
  
    
  }


  		




  29
  31
  
    

  		












Also available in:  Unified diff