Beginners: please read this post and this post before posting to the forum.
0 Members and 1 Guest are viewing this topic.
#include "sys/axon2.h"#include "servos.h"#include "a2d.h"#include "sensors/distance/sharp/GP2.h" // Define one distance sensors connected to ADC channels 0#define sensordistance ADC_NUMBER_TO_CHANNEL(0) // Define three servosSERVO left = MAKE_SERVO(FALSE, B5,1500, 500);SERVO right = MAKE_SERVO(TRUE, B6,1500, 500);SERVO scan = MAKE_SERVO(FALSE, B7,1500, 500);// Define IR sensorSharp_GP2D12 sensor = MAKE_Sharp_GP2D12(ADC0); // Create the list - remember to place an & at the// start of each servo name and to use PROGMEMSERVO_LIST PROGMEM servos[] = {&left,&right,&scan}; // Create a driver for the list of servosSERVO_DRIVER bank1 = MAKE_SERVO_DRIVER(servos); //the larger this number, the more likely your robot will drive straight #define threshold 8 // In my initialising code - pass the list of servos to controlvoid appInitHardware(void){ // Initialise the servo controller servoPWMInit(&bank1); // Initialise the sensor distanceInit(sensor); // Give each servo a start value of 'stop' act_setSpeed(&left, 0); act_setSpeed(&right,0); act_setSpeed(&scan, 0); }TICK_COUNT appInitSoftware(TICK_COUNT loopStart){ return 0;} // This is the main loopTICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) { // This is for sensor data distanceRead(sensor); int sharp_IR_reading=0;int scan_thresh=0;//threshold value of scanning sensorint scan_angle=20;//position of scanner, in units of servo commandint max_scan_angle=56;//maximum position scanner can rotate to (57)//this function causes scanning servo to center on edge of objectvoid scan(void) { sharp_IR_reading=a2dConvert8bit(0); if (sharp_IR_reading > scan_thresh)//object detected { if (scan_angle>41) //overflow protection scan_angle-=2;//scan right } else //object not detected { if (scan_angle<=max_scan_angle) //maximum servo angle scan_angle+=2; //scan left //else //if scanned all the way, this forces it to start over // scan_angle=30; } } //automatically calculates threshold value before runvoid autocalibrate(void) { scan_thresh=a2dConvert8bit(0);//sensor reading } if(scan_angle > 57){ // go left act_setSpeed(&left,DRIVE_SPEED_MIN); act_setSpeed(&right,DRIVE_SPEED_MAX); }else if(scan_angle < 41){ // go right act_setSpeed(&left,DRIVE_SPEED_MAX); act_setSpeed(&right,DRIVE_SPEED_MIN); }else{ // Go forwards act_setSpeed(&left,DRIVE_SPEED_MAX); act_setSpeed(&right,DRIVE_SPEED_MAX); } return 20000; // wait for 20ms to stop crazy oscillations }