void init_motor(void) {
set_motor(0,0);
set_motor(1,0);
DDRB |= _BV(MOTOR0_EN_PIN) | _BV(MOTOR1_EN_PIN);
DDRD |= _BV(MOTOR0_DIR0_PIN) | _BV(MOTOR0_DIR1_PIN) | _BV(MOTOR1_DIR0_PIN) | _BV(MOTOR1_DIR1_PIN);
}
int main()
{
int i;
init(0);
// connect camera to the screen
open_screen_stream();
// set all didgital outputs to +5V
for (i = 0; i < 8; i++)
{
// set all digital channels as outputs
select_IO(i,0);
write_digital(i,1);
}
while(1)
{
motorControl(line());
}
// terminate hardware
close_screen_stream();
set_motor(1,0);
set_motor(2,0);
return 0;
}
static void mode_motortest(void)
{
set_motor(MOTOR_ON);
_delay_ms(400);
set_motor(MOTOR_OFF);
_delay_ms(500);
return;
}
//stops motors spinning
void handle_signal(int signal){
if(signal == SIGINT){
set_motor(1, 0);
set_motor(2, 0);
std::exit(0);
}
}
//This method we got off the web, it stops the program when ctrl+c is pressed.
void signal_callback_handler(int signum) {
//We caught sig 2 (ctrl+c)
printf("Caught signal %d\n",signum);
//Kill motors
set_motor(1,0);
set_motor(2,0);
// Terminate program
exit(signum);
}
int main() {
init(0);
//Left is 1, right is 2
//do both motors run at the same speed? Or will we need to change one of the values?
//We will need to change a value to get it right
set_motor(1,255);
set_motor(2,255);
Sleep(3,0); //Will this be included in the final code? Or is it just for debugging purposes?
set_motor(1,0);
set_motor(2,0);
}
/***************************************************************
Set the current state of drive motors
***************************************************************/
void set_drive_motors(int power1, int power2)
{
set_motor(1, _dec(power1));
set_motor(2, _dec(power2));
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x01);
push_char(0x60);
send_checksum();
}
int main(){
init(0);
for(int i=0; i<5; i++){
set_motor(1,60);
sleep(2,0);
set_motor(1,0);
set_motor(2,60);
sleep(2,0);
set_motor(2,0);
sleep(2,0);
}
return 0;}
/***************************************************************
Set the forward speed and turn rate
***************************************************************/
void set_speed_and_turn(int spd, int turn)
{
int speed = _dec(spd);
int turnrate = _dec(turn);
set_motor(1, 0);
set_motor(2, 0);
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x01);
push_char(0x61);
send_checksum();
}
/*****************************************************************
* int set_motor_all(float duty)
*
* applies the same duty cycle argument to all 4 motors
*****************************************************************/
int set_motor_all(float duty){
int i;
for(i=1;i<=MOTOR_CHANNELS; i++){
set_motor(i, duty);
}
return 0;
}
int main(){
init(1);
int x;
connect_to_server("130.195.6.196", 1024); //Opening the gate code
send_to_server("Please");
char message[24];
receive_from_server(message);
printf("%s", message);
send_to_server(message);
for (x = 0; x < 8; x++)
{
select_IO(x,0);
write_digital(x,1);
}
while(1){
take_picture();
int sum = 0;
float kp = 0.5;
int w;
int s;
int proportional_signal=0;
for(int x = 0; x < 320; x++){
w = get_pixel(x,120,3);
if(w>127){s=1;}
else{s=0;};
sum = sum + (x-160)*s;}
proportional_signal = sum * kp;
if (sum < 0){
set_motor(1, proportional_signal);
set_motor(2, -1*proportional_signal);
}
else if (sum > 0){
set_motor(1, -1*proportional_signal);
set_motor(2, proportional_signal);
}
else{
set_motor(1, proportional_signal);
set_motor(2, proportional_signal);
}
update_screen();
for (x = 0 ; x < 8; x++)
{
int adc_reading = read_analog(x);
printf("ai=%d av=%d\n",x,adc_reading);
}
}
set_motor(1,0);
set_motor(2,0);
return 0;
}
static void blinkrattlebeep(void){
led_off(LED_L|LED_R);
_delay_ms(100);
led_on(LED_L);
_delay_ms(100);
led_off(LED_L);
led_on(LED_R);
_delay_ms(100);
led_off(LED_R);
set_motor(MOTOR_ON);
_delay_ms(200);
set_motor(MOTOR_OFF);
buzzr_up();
for (int i=0; i<100; i++){
_delay_ms(2);
buzzr_inv();
};
buzzr_off();
}
void maze(){
// maze code
int prev_error = 0;
int total_error = 0;
while(true){
int ir_sensor_left = read_analog(2);
int ir_sensor_right = read_analog(1);
int ir_sensor_forward = read_analog(0);
printf("Left:%d\n", ir_sensor_left);
printf("Right:%d\n", ir_sensor_right);
printf("Ford:%d\n", ir_sensor_forward);
int error_signal = ir_sensor_right - ir_sensor_left;
total_error += error_signal;
// see PID control https://github.com/kaiwhata/ENGR101-2016/wiki/PID-(Proportional-Integral-Derivative)-Control
double proportional_signal = error_signal*MAZE_KP;
double derivative_signal = (error_signal-prev_error/0.1)*MAZE_KD;
double integral_signal = total_error*MAZE_KI;
prev_error = error_signal;
int total_signal = proportional_signal + derivative_signal + integral_signal;
// robot will always turn right
if(ir_sensor_right < 130){
printf("hi\n");
set_motor(1, MOTOR_SPEED);
set_motor(2, -MOTOR_SPEED);
Sleep(0, 800000);
set_motor(1, MOTOR_SPEED);
set_motor(2, 0);
Sleep(1,500000);
ir_sensor_right = read_analog(2);
/*if(ir_sensor_right < 130){
set_motor(1, MOTOR_SPEED);
set_motor(2, -MOTOR_SPEED);
Sleep(0, 500000);
set_motor(1, MOTOR_SPEED);
set_motor(2, 0);
Sleep(1,0);
}*/
} else if(ir_sensor_forward > 300){
// spins robot around
set_motor(1, -MOTOR_SPEED);
set_motor(2, -MOTOR_SPEED);
} else {
// goes straight forward
set_motor(1, MOTOR_SPEED - total_signal);
set_motor(2, -MOTOR_SPEED - total_signal);
}
Sleep(0,SLEEP_TIME);
}
return;
}
/***************************************************************
Set the state of single motor
***************************************************************/
void set_single_motor(int id, int power)
{
set_motor(id, _dec(power));
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x01);
push_char(0x10 + id);
send_checksum();
}
int32_t CMotor::motor_init()
{
unsigned int i;
for (i = 0; i < MOTORS_COUNT; i++)
set_motor(i, 0);
motor_refresh();
return 0;
}
void deadendbackup()
{
while (front<50 && (left<50 || right<50))
{
set_motor(MOTOR_LEFT, (-SPEED_DEF));
set_motor(MOTOR_RIGHT, (-SPEED_DEF));
}
int left_speed = SPEED_DEF - amount;
int right_speed = SPEED_DEF + amount;
if((left > 50))
{
set_motor(MOTOR_LEFT, left_speed);
}
else if(right > 50)
{
set_motor(MOTOR_RIGHT, right_speed);
}
}
int motorControl(double error_signal)
{
printf(" error signal: %f\n",error_signal);
int SPEED = 80;
double modSpeed;
//if too far left
if(error_signal < 0){
modSpeed = speedCheck(0, SPEED, SPEED-error_signal);
set_motor(2,SPEED);//right motor
set_motor(1,SPEED/2);//left motor
printf("Too far left!\n");
printf("Left motor: %d Right motor %d\n",SPEED, modSpeed);
}
//if too far right
else if(error_signal > 0){
modSpeed = speedCheck(0, SPEED, SPEED-error_signal);
set_motor(2,SPEED/2);
set_motor(1,SPEED);
printf("Too far right!\n");
printf("Left motor: %d Right motor %d\n", modSpeed, SPEED);
}
//if centered
else{
set_motor(1,SPEED);
set_motor(2,SPEED);
printf("Going straight\n");
}
return 0;
}
/**
* ggrbug mode
* - simulate geiger counter sounds
*/
static void geiger(void)
{
uint8_t max = 200;
uint8_t min = 10;
static timer_t mytimer;
static bool blink;
if (mode_uninitialized) {
init_leds();
music_setNote(NOTE_PAUSE, 0);
mode_uninitialized = false;
timer_set(&mytimer, 10);
blink = false;
};
if (timer_expired(&mytimer)) {
if (!blink) {
/*lets blink*/
int i = (rand() % 3);
switch (i) {
case 0: led_on(LED_L); break;
case 1: led_on(LED_R); break;
default: led_on(LED_L | LED_R); break;
};
if (rand() % 10 > 8)
set_motor(MOTOR_ON);
music_setNote(NOTE_C, 5);
timer_set(&mytimer, 2);
blink = true;
} else {
/*stop blink*/
led_off(LED_L | LED_R);
set_motor(MOTOR_OFF);
music_setNote(NOTE_PAUSE, 0);
timer_set(&mytimer, (rand() % (max - min)) + min);
blink = false;
}
}//end if timer_expired
}
double line() {
int sum = 0;
double kp = 0.8; //example value, testing needed
int colourVal, s;
int SPEED=80;
double proportional_signal;
take_picture(); // take camera shot
for(int col=0; col < 320; col++){
colourVal=get_pixel(col, 120, 3);
if(colourVal>127){s=1;}//if it's closer to white
else if(colourVal<127){
set_motor(2,-1*SPEED);//right motor
set_motor(1,-1*SPEED);//left motor
Sleep(1,0);
}
sum = sum + (col-160)*s;
}
update_screen();
proportional_signal = sum*kp;
return proportional_signal;
}
int main() {
init(0);
int count = 0;
int proportional_signal = 1;
while (count < 2000) {
proportional_signal = getLineValue();
if (proportional_signal < 0) {
rightMotor = (50 - (proportional_signal / (160)) * 175);
leftSpeed = (50 + (proportional_signal / (160)) * 175);
}
else if (proportional_signal > 0) {
rightMotor = (50 - (proportional_signal / (160)) * 175);
leftMotor = (50 + (proportional_signal / (160)) * 175);
}
else {
leftMotor = -80;
rightMotor = -80;
}
printf("%d\n", leftMotor);
printf("%d\n", rightMotor);
set_motor(1, leftMotor);
set_motor(2, rightMotor);
count++;
}
return 0;
}
// Close Serial Port
void
close_serial()
{
// printf("Not Releasing channels (Commented out)\n");
printf("Setting servo and motor to 1500");
set_servo(1500);
set_motor(1500);
printf("Releasing channels");
release_channels();
printf("CLOSE PORT\n");
int result = close(fd);
if ( result )
{
fprintf(stderr,"WARNING: Error on port close (%i)\n", result );
}
status = 0;
printf("\n");
}
int main(){
//This sets up the RPi hardware and ensures
//everything is working correctly
init(0);
//keep going till line finished or no line
while(true){
//Take picture with camera
take_picture();
preError = avError;
counter = 0;
error = 0;
for (int i=0; i<320; i++){
w = get_pixel(i, 120, 3);
if(w > 100){
error += (i-160);
counter++;
}
}
//rests for 0.1 seconds
Sleep(0,(1000000*sleepTime));
if(counter!=0){
//Proportional Signal
avError = error/counter;
propSignal = (avError/maxP)*Kp;
if(propSignal>Kp){ //ensures the propSignal between 0 - Kp
propSignal = Kp;
}
//Derivative Signal
deltaError = (avError - preError)/(sleepTime);
//x should be the maximum value of deltaError
//trial and error find an x value that works
derSignal = (deltaError/maxD)*Kd;
if(derSignal>Kd){ //ensures the derSignal between 0 - Kd
printf("derSignal: %d", derSignal);
derSignal = Kd;
}
//not sure which wheel is which
//assumes positive is forwards
//should be right wheel
set_motor(1, ((maxSpeed - (Kp + Kd)) - (propSignal + DerSignal)));
//should be left wheel
set_motor(2, ((maxSpeed - (Kp + Kd)) + (propSignal + DerSignal)));
}else{
//turns until line is found.
set_motor(1, -45);
set_motor(2, 40);
}
}
return 0;
}
void write_car_state(struct car* c) {
(c->motor).left = c->speed * MIN(100, (100 - c->direction)) / 100;
(c->motor).right = c->speed * MIN(100, (100 + c->direction)) / 100;
set_motor(&c->motor);
}
int main(){
//VARIABLE INITIALIZATION
//Networking
char message[24];
//Line Following
char c;
float kp = 0.80;
float ki = 0.02;
float kd = 0.04;
int i;
int leftCheck;
int frontCheck;
int rightCheck;
bool left, front, right;
int whiteTotal, prevWhiteLocation, whiteLocation;
double whiteRatio;
double prevRatio;
double derivWhite;
double integWhite;
//Maze
int leftSensor, rightSensor;
int whiteWall;
bool noLeftWall, noRightWall, noWallAhead;
int THRESHOLD = 250; //Sensor Threshold
int totalWidth;
//Primary Initialization
init(1);
//Networking Section
//Send Signal to open gate
connect_to_server("130.195.6.196", 1024);
send_to_server("please");
receive_from_server(message);
send_to_server(message);
//Line Following Section
//Loop runs until both sensors sense walls (start of maze)
while((read_analog(0) < THRESHOLD) || (read_analog(1) < THRESHOLD)){
//Set variables
left = false;
front = false;
right = false;
whiteTotal = 0;
leftCheck = 0;
frontCheck = 0;
rightCheck = 0;
//Take readings
take_picture();
for(i = 0; i < 240; i++){
c = get_pixel(40, i, 3);
if(c > 120){
whiteTotal++;
whiteLocation = whiteLocation + (i-120);
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 60, 3);
if(c > 120){
leftCheck++;
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 180, 3);
if(c > 120){
rightCheck++;
}
}
for(i = 30; i < 210; i++){
c = get_pixel(160, i, 3);
if(c > 120){
frontCheck++;
}
}
if(leftCheck > 5){
left = true;
}
if(frontCheck > 10){
front = true;
}
if(rightCheck > 5){
right = true;
}
if(left){
set_motor(1, 50);
set_motor(2, 0);
Sleep(0, 500000); //Left Sleep
}
else if(front && right){
set_motor(1, 50);
set_motor(2, -50);
Sleep(0, 500000); //Front Sleep
}
else if(right){
set_motor(1, 0);
set_motor(2, -50);
Sleep(0, 500000); //Right Sleep
}
else if(whiteTotal < 1){
set_motor(1, -50);
set_motor(2, 50);
Sleep(0, 100000); //Turn around Sleep
}
else{
derivWhite = ((double)whiteLocation - (double)prevWhiteLocation)/0.01;
integWhite = integWhite + ((double)whiteLocation * 0.01);
whiteLocation = whiteLocation/whiteTotal;
// set_motor(1, ((int) ((-(whiteLocation*40/120)*kp+kd*derivWhite)+40)));
// set_motor(2, -((int) (((whiteLocation*40/120)*kp+kd*derivWhite)+40)));
set_motor(1, ((int)(-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))));
set_motor(2, -((int) (((whiteLocation*40/120)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)));
prevWhiteLocation = whiteLocation;
Sleep(0,1000);
}
}
while(true){
}
return 0;
}
int main(){
//This sets up the RPi hardware and ensures
//everything is working correctly
//Initializing variables
init(0);
int w;
int s;
double P = 0;
double D = 0;
double kp = 0.31;
double kd = 0.003;
int maxSpeed = 40;
int VL;
int VR;
time_t start; //Sets 0 Point for timer to begin the clock at.
time_t finish; //Sets finishing time
int dif;
int count = 0;
double tim = 0.03;
double avgC = 0;
double current_error = 0;
double previous_error = 0;
double delta_error;
int setTime = 0;
bool line = true;
int iCount = 0;
double lastCurrent_error = 0;
bool white_light;
//------------------------------------------------------------------------------
while(true){
start = time(NULL);//Starts Timer
take_picture();
white_light = true;
//--------------------------------------Analyzing Image-------------------------
for (int i = 0; i < 320; i++){
w = get_pixel(i,120,3);
if(w < 127){ //If pixel is close to black ------ (Reduces noise)
s=0;
}else {
s=1;
white_light = false;
}
current_error = current_error + (i-160)*s; //Adds to current_error if its a white pixel
}//Closes For Loop
//-------------------------------------No Line Detected-------------------------
if(current_error == 0 && white_light){ //error is 0 and black_light
line = false;
VL = maxSpeed;
VR = maxSpeed;
set_motor(1,-VL);//Sets motors to reverse
set_motor(2,VR);
iCount++; //Counts number of frames line has been missing for
}else{
line = true;
iCount = 0;
}
if(iCount>0){ //Debugging
printf("Count = %d (line has been missing for %d FPS)\n",iCount,iCount);
}
if( current_error<0 || current_error>0 ){
lastCurrent_error = current_error;
}
if(iCount>7){ //Line has been lost for a while. Run Correction
if(lastCurrent_error>0){
set_motor(1,-VL);
set_motor(2,-VR);
Sleep(0,100000);
}else if(lastCurrent_error<0){
set_motor(1,VL);
set_motor(2,VR);
Sleep(0,100000);
}
}//Closes iCount if statement
//--------------------------------------Time Stamp & FPS------------------------
finish = time(NULL);
dif = finish -start;
if(dif==1){// Only occurs when 1 second has passed when 1 second has passed.
tim = 1/count; //Updates the current time taken for AVC to process 1 Frame
count = 0;
}
//---------------------------------------Error Values---------------------------
avgC = avgC + (current_error/320);
P = (current_error/320)*kp;
//printf("D = %f\nP = %f\n",D,P); //Debugging
current_error = 0; //Resets current error to 0
setTime++;
count++;
//----------------------------- D value for PID --------------------------------
if(setTime == 1){ //This means our D value changes every frame
D = ((avgC-previous_error)/(0.03))*kd;
previous_error = avgC;
avgC = 0;
setTime = 0;
}
//----------------------------------------Motor Control-------------------------
if(line){
VL = maxSpeed - (P) + (D);
VR = maxSpeed + (P) - (D);
set_motor(1,VL);
set_motor(2,-VR);
}
//-----------------------------------------END OF PROGRAM-----------------------
}//Closes While Loop
return 0;}
/***********************************************************************
* drive_stack
* This is the medium between the user_interface struct and the
* physical servos and motors
************************************************************************/
void* drive_stack(void* ptr){
int i; // general purpose counter for for loops
float net_drive, net_turn, net_torque_split;
// exiting condition is checked inside the switch case instead
while(1){
switch (get_state()){
case EXITING:
return NULL;
case PAUSED:
disable_motors();
// not much to do if paused!
break;
// when running, drive_stack checks if an input mode
// like mavlink, DSM2, or bluetooth is enabled
// and moves the servos and motors corresponding to
// user input and current controller mode
case RUNNING:
if(user_interface.input_mode == NONE){
cstate.servos[0]=config.serv1_center-config.turn_straight;
cstate.servos[1]=config.serv2_center+config.turn_straight;
cstate.servos[2]=config.serv3_center-config.turn_straight;
cstate.servos[3]=config.serv4_center+config.turn_straight;
for (i=1; i<=4; i++){
send_servo_pulse_normalized(i,cstate.servos[i-1]);
}
disable_motors();
break;
}
enable_motors();
// now send input to servos and motors based on drive mode and UI
// motors 2 and 3 have negative polarity in the config.txt file so that positive sign in this file =
// clockwise spin with a forward input. Eg. all of the net_drive values are positive in spin mode.
// 1 3 0 2 New version is 1 2 0 1 to match the orientation of motor wire
// 2 4 1 3 4 3 3 2 connectors on cape
switch(user_interface.drive_mode){
case LANECHANGE: // lane change maneuver
net_drive = user_interface.drive_stick*config.motor_max;
net_turn = user_interface.turn_stick*(0.5-config.turn_straight);
cstate.motors[0]=net_drive*config.mot1_polarity;
cstate.motors[1]=net_drive*config.mot2_polarity;
cstate.motors[2]=net_drive*config.mot3_polarity;
cstate.motors[3]=net_drive*config.mot4_polarity;
cstate.servos[0]=config.serv1_center-config.turn_straight+net_turn;
cstate.servos[1]=config.serv2_center+config.turn_straight+net_turn;
cstate.servos[2]=config.serv3_center-config.turn_straight+net_turn;
cstate.servos[3]=config.serv4_center+config.turn_straight+net_turn;
break;
case NORMAL_4W: // Normal 4W Steering
net_drive = user_interface.drive_stick*config.motor_max;
net_turn = user_interface.turn_stick*config.normal_turn_range;
net_torque_split = user_interface.turn_stick*config.torque_vec_const*net_drive;
cstate.motors[0]=(net_drive+net_torque_split)*config.mot1_polarity;
cstate.motors[1]=(net_drive+net_torque_split)*config.mot2_polarity;
cstate.motors[2]=(net_drive-net_torque_split)*config.mot3_polarity;
cstate.motors[3]=(net_drive-net_torque_split)*config.mot4_polarity;
cstate.servos[0]=config.serv1_center-config.turn_straight+net_turn;
cstate.servos[1]=config.serv2_center+config.turn_straight+net_turn;
cstate.servos[2]=config.serv3_center-config.turn_straight-net_turn;
cstate.servos[3]=config.serv4_center+config.turn_straight-net_turn;
break;
// crab, turn all wheels sideways and drive
case CRAB:
net_drive = user_interface.drive_stick*config.motor_max;
net_turn = user_interface.turn_stick*config.crab_turn_const\
*(net_drive+0.5);
cstate.motors[0]=(net_drive+net_turn)*config.mot1_polarity;
cstate.motors[1]=-(net_drive+net_turn)*config.mot2_polarity;
cstate.motors[2]=-(net_drive-net_turn)*config.mot3_polarity;
cstate.motors[3]=(net_drive-net_turn)*config.mot4_polarity;
cstate.servos[0]=config.serv1_center+config.turn_crab;
cstate.servos[1]=config.serv2_center-config.turn_crab;
cstate.servos[2]=config.serv3_center+config.turn_crab;
cstate.servos[3]=config.serv4_center-config.turn_crab;
break;
case SPIN:
net_drive = user_interface.turn_stick*config.motor_max;
cstate.motors[0]=net_drive*config.mot1_polarity;
cstate.motors[1]=-net_drive*config.mot2_polarity;
cstate.motors[2]=-net_drive*config.mot3_polarity;
cstate.motors[3]=net_drive*config.mot4_polarity;
cstate.servos[0]=config.serv1_center+config.turn_spin;
cstate.servos[1]=config.serv2_center-config.turn_spin;
cstate.servos[2]=config.serv3_center+config.turn_spin;
cstate.servos[3]=config.serv4_center-config.turn_spin;
break;
default:
printf("unknown drive_mode\n");
disable_motors();
for (i=1; i<=4; i++){
cstate.motors[i-1]=0;
cstate.servos[i-1]=0.5;
}
break;
}// end of switch(drive_mode)
// send pulses to servos and drive motors
for (i=1; i<=4; i++){
saturate_number(&cstate.servos[i-1],config.turn_min,config.turn_max);
saturate_number(&cstate.motors[i-1],-config.motor_max,config.motor_max);
set_motor(i,cstate.motors[i-1]);
send_servo_pulse_normalized(i,cstate.servos[i-1]);
}
default:
break;
} // end of switch get_state()
// run about as fast as the core itself
usleep(1000000/CONTROL_HZ);
}
return NULL;
}
int main (){
// This sets up the RPi hardware and ensures
// everything is working correctly
init(0);
// int arrayOfPixels[64]; //For every 5 pixels (340/5 = 64)
// int total = 0;
float kd = 1.2;
// float ki = 0.5;
// float kd = 0.2;
// float total_error = 0;
float current_error = 0;
// int previous_error = 0;
// int error_diff = 0;
// int error_period = 0; //need to change
float pid;
int w, s;
double proportional_signal;
// int integral_signal = 0;
// int derivative_signal = 0;
// for(i = 0; i < 8; i++){
// select_IO(i, 0);
// write_digital(i, 1);
// }
while (1) {
take_picture();
int color = get_pixel(102, 55, 3);
// printf("Color=%d\n", color);
// int adc_reading = 0;
current_error = 0;
proportional_signal = 0;
// for(i = 0; i < 64; i++){
//need to check if 120 is the middle or not
// arrayOfPixels[i] = get_pixel(120,i*5, 3);
//Getting total of the pixels along the pane
// total = total + arrayOfPixels[i];
// }
for(int i=0; i<320; i++){
w = get_pixel(i,120,3);
// printf("%d\n",w);
if(w>120){
s = 1;
}else{
s = 0;
}
//if to right, positive
current_error = (current_error + (i-160)*s);
// printf("current_error is %f \n", current_error);
}
// printf("PID: %f \n", pid);
//Random error checking code, most of which isn't implemented yet
// total_error = total_error + current_error;
// integral_signal = total*ki;
// error_diff = current_error-previous_error;
// derivative_signal = (error_diff/error_period)*kd;
// previous_error = current_error;
//Getting the robot to move
proportional_signal = current_error*kd;
// printf("Current Error: %f \n", current_error);
// printf("Proportional Signal %f \n", proportional_signal);
/** int abc;
if(proportional_signal > 0){
abc = 1;
}else if(proportional_signal < 0){
abc = -1;
}else{
abc = 1;
}*/
pid =( proportional_signal );
printf("PID: %f \n", pid);
if(pid > 255){
set_motor(1,-1*255);
set_motor(2, 255);
}else if(pid < -255){
set_motor(1, 255);
set_motor(2,- 1*255);
}else{
set_motor(1, -pid);
set_motor(2, -pid);
}
}
return 0;
}
/******************************************************************
* balance_core()
* discrete-time balance controller operated off IMU interrupt
* Called at SAMPLE_RATE_HZ
*******************************************************************/
int balance_core(){
// local variables only in memory scope of balance_core
static int D1_saturation_counter = 0;
float compensated_D1_output = 0;
float dutyL = 0;
float dutyR = 0;
static log_entry_t new_log_entry;
float output_scale; //battery voltage/nominal voltage
// if an IMU packet read failed, ignore and just return
// the mpu9150_read function may print it's own warnings
if (mpu9150_read(&mpu) != 0){
return -1;
}
/**************************************************************
* STATE_ESTIMATION
* read sensors and compute the state regardless of if the
* controller is ARMED or DISARMED
***************************************************************/
// angle theta is positive in the direction of forward tip
// add mounting angle of BBB
cstate.theta[2] = cstate.theta[1]; cstate.theta[1] = cstate.theta[0];
cstate.theta[0] = mpu.fusedEuler[VEC3_X] + config.bb_mount_angle;
cstate.current_theta = cstate.theta[0];
cstate.d_theta = (cstate.theta[0]-cstate.theta[1])/DT;
// collect encoder positions
cstate.wheelAngleR = -(get_encoder_pos(config.encoder_channel_R) * 2*PI) \
/(config.gearbox * config.encoder_res);
cstate.wheelAngleL = (get_encoder_pos(config.encoder_channel_L) * 2*PI) \
/(config.gearbox * config.encoder_res);
// log phi estimate
// wheel angle is relative to body,
// add theta body angle to get absolute wheel position
cstate.phi[2] = cstate.phi[1]; cstate.phi[1] = cstate.phi[0];
cstate.phi[0] = ((cstate.wheelAngleL + cstate.wheelAngleR)/2) +cstate.current_theta;
cstate.current_phi = cstate.phi[0];
cstate.d_phi = (cstate.phi[0]-cstate.phi[1])/DT;
// body turning estimation
cstate.gamma[2] = cstate.gamma[1]; cstate.gamma[1] = cstate.phi[0];
cstate.gamma[0]=(cstate.wheelAngleL-cstate.wheelAngleR) \
* (config.wheel_radius/config.track_width);
cstate.d_gamma = (cstate.gamma[0]-cstate.gamma[1])/DT;
cstate.current_gamma = cstate.gamma[0];
// output scaling
output_scale = cstate.vBatt/config.v_nominal;
/*************************************************************
* Control based on the robotics_library defined state variable
* PAUSED: make sure the controller stays DISARMED
* RUNNING: Normal operation of controller.
* - check for tipover
* - wait for MiP to be within config.start_angle of
* upright
* - choose mode from setpoint.core_mode
* - evaluate difference equation and check saturation
* - actuate motors
***************************************************************/
switch (get_state()){
// make sure things are off if program is closing
case EXITING:
disable_motors();
return 0;
// if the controller is somehow still ARMED, disarm it
case PAUSED:
if(setpoint.arm_state==ARMED){
disarm_controller();
}
break;
// normal operating mode
case RUNNING:
// exit if the controller was not armed properly
if(setpoint.arm_state==DISARMED){
return 0;
}
// check for a tipover before anything else
if(fabs(cstate.current_theta)>config.tip_angle){
disarm_controller();
printf("tip detected \n");
break;
}
/**********************************************************
* POSITION Phi controller
* feedback control of wheel angle Phi by outputting a
* reference theta body angle. This is controller D2 in
* config
***********************************************************/
if(setpoint.core_mode == POSITION){
// move the position set points based on user input
if(setpoint.phi_dot != 0.0){
//setpoint.phi == cstate.current_phi + setpoint.phi_dot*DT;
setpoint.phi += setpoint.phi_dot * DT;
}
// march the different equation terms for the input Phi Error
// and the output theta reference angle
cstate.ePhi[2] = cstate.ePhi[1];
cstate.ePhi[1] = cstate.ePhi[0];
cstate.ePhi[0] = setpoint.phi-cstate.current_phi;
cstate.theta_ref[2] = cstate.theta_ref[1];
cstate.theta_ref[1] = cstate.theta_ref[0];
// evaluate D2 difference equation
cstate.theta_ref[0] = config.K_D2*( \
config.numD2_2 * cstate.ePhi[2] \
+ config.numD2_1 * cstate.ePhi[1] \
+ config.numD2_0 * cstate.ePhi[0]) \
-(config.denD2_2 * cstate.theta_ref[2] \
+ config.denD2_1 * cstate.theta_ref[1]);
//check saturation of outer loop theta reference output signal
saturate_float(&cstate.theta_ref[0],-config.theta_ref_max,config.theta_ref_max);
setpoint.theta = cstate.theta_ref[0];
}
// evaluate inner loop controller D1z
cstate.eTheta[2] = cstate.eTheta[1];
cstate.eTheta[1] = cstate.eTheta[0];
cstate.eTheta[0] = setpoint.theta - cstate.current_theta;
cstate.u[2] = cstate.u[1];
cstate.u[1] = cstate.u[0];
cstate.u[0] = \
config.K_D1 * (config.numD1_0 * cstate.eTheta[0] \
+ config.numD1_1 * cstate.eTheta[1] \
+ config.numD1_2 * cstate.eTheta[2]) \
- (config.denD1_1 * cstate.u[1] \
+ config.denD1_2 * cstate.u[2]);
// check saturation of inner loop knowing that right after
// this control will be scaled by battery voltage
if(saturate_float(&cstate.u[0], -output_scale, output_scale)){
D1_saturation_counter ++;
if(D1_saturation_counter > SAMPLE_RATE_HZ*config.pickup_detection_time){
printf("inner loop controller saturated\n");
disarm_controller();
D1_saturation_counter = 0;
break;
}
}
else{
D1_saturation_counter = 0;
}
cstate.current_u = cstate.u[0];
// scale output to compensate for battery charge level
compensated_D1_output = cstate.u[0] / output_scale;
// // integrate the reference theta to correct for imbalance or sensor
// // only if standing relatively still with zero phi reference
// // to-do, wait for stillness for a time period before integrating
// if(setpoint.phi==0 && fabs(cstate.phi_dot)<2){
// state.thetaTrim += (config.kTrim*cstate.theta_ref[0]) * DT;
// }
//steering controller
// move the controller set points based on user input
setpoint.gamma += setpoint.gamma_dot * DT;
cstate.egamma[1] = cstate.egamma[0];
cstate.egamma[0] = setpoint.gamma - cstate.current_gamma;
cstate.duty_split = config.KP_steer*(cstate.egamma[0] \
+config.KD_steer*(cstate.egamma[0]-cstate.egamma[1]));
// if the steering input would saturate a motor, reduce
// the steering input to prevent compromising balance input
if(fabs(compensated_D1_output)+fabs(cstate.duty_split) > 1){
if(cstate.duty_split > 0){
cstate.duty_split = 1-fabs(compensated_D1_output);
}
else cstate.duty_split = -(1-fabs(compensated_D1_output));
}
// add D1 balance controller and steering control
dutyL = compensated_D1_output - cstate.duty_split;
dutyR = compensated_D1_output + cstate.duty_split;
// send to motors
// one motor is flipped on chassis so reverse duty to L
set_motor(config.motor_channel_L,-dutyL);
set_motor(config.motor_channel_R,dutyR);
cstate.time_us = microsSinceEpoch();
// pass new information to the log with add_to_buffer
// this only puts information in memory, doesn't
// write to disk immediately
if(config.enable_logging){
new_log_entry.time_us = cstate.time_us-core_start_time_us;
new_log_entry.theta = cstate.current_theta;
new_log_entry.theta_ref = setpoint.theta;
new_log_entry.phi = cstate.current_phi;
new_log_entry.u = cstate.current_u;
add_to_buffer(new_log_entry);
}
// end of normal balancing routine
// last_state will be updated beginning of next interrupt
break;
default:
break; // nothing to do if UNINITIALIZED
}
return 0;
}
int move(){
int mtrSp = 50;
int check = 0;
int white_threshold = 130;//Threshold of white, i.e. from the 0 to 255 only values above this are detected
float kP = 0.2;//Prop constant which scales with error signal
float kD = 0.00015;
int pastError = 0;//Past error to work out kD
int currentError = 0;//Absolute of error signal - will need to check that works
int eValue = 0;//Average value of error either side
//Sleep(2,0); //For gate
while(1){
int totalSum = 0;
int pSignal = 0;
int dSignal = 0;
int sum = 0;
int num = 0;
int eValue = 0;
int w = 0;
int left = 0; //True if line is left
int right = 0; //True if line is right
int top = 0; //True if line is forward
int leftSum = 0; //Totals amount of left mid pixels which are white
int rightSum = 0; //Totals amount of right mid pixels which are white
int topSum = 0; //Totals amount of top mid pixels which are white
//Totals amount of top mid pixels which are white
take_picture();
for(int i = 0; i < 200; i++){ //For loop to save pixels to arrays and test whiteness, iterates through from a base value to reach a max
//For left and right, this is from row 100 to row 215, column 1 and 319 respectively
//For top this is from
int leftSide = get_pixel(40, i, 3);//Saves the value of the left-mid pixels if above threshold
if(leftSide > 130){
leftSum = leftSum + 1;//Adds to a total count of pixels that are white
}else{//If not valid pixel skip
leftSum = leftSum + 0;
}
int rightSide = get_pixel(280, i, 3);//Saves the value of the right-mid pixels if above threshold
if(rightSide > 130){
rightSum = rightSum + 1;//Adds to a total count of pixels that are white
}else{//If not valid pixel skip
rightSum = rightSum + 0;
}
}
if(leftSum > 40){
left = 1;
}else{
left = 0;
}
if(rightSum > 40){
right = 1;
}else{
right = 0;
}
if(topSum > 20){
top = 1;
}else{
top = 0;
}
for(int i = 0; i < 320; i++){ /**Less than 320 as the image is 320 pixels across*/
sum = get_pixel(i, 1, 3);//Gets pixel at row 1 as it goes from 1 to 240
if(sum > 130){ //If value greater than threshold make it 1 and add to num
w = 1;
num++; //num increases when a white pixel is found
topSum = topSum + 1;
}else{
w = 0;
}
totalSum = totalSum + ((i - 160) * w);//Takes the position of the i and adds to a total
}
printf("num %d\n", num);
if(num > 310){
int check = check + 1;
printf("Check %d\n", check);
//mtrSp = 40;
if(check > 6){
if(left == 1 && right == 1){ //T intersection (choose left)
set_motor(1, 0);
set_motor(2, -55);
Sleep(0, 500000);
//set_motor(1, 0);
//set_motor(2, 0);
}else if(left == 0 && right == 1){ //Right side turn
printf("Right %d\n", right);
set_motor(1, 55);
set_motor(2,0);
Sleep(0, 500000);
//set_motor(2,0);
//set_motor(1, 0);
}else if(num < 20){ //If not enough pixels are found, reverse and reset
set_motor(1, -40.5);
set_motor(2, 40);
Sleep(0, 50000);
continue;
}
}
}else if(num < 20){ //If not enough pixels are found, reverse and reset
set_motor(1, -40.5);
set_motor(2, 40);
Sleep(0, 50000);
continue;
}else{
eValue = totalSum/num;//Finds average of a point at
pSignal = eValue*kP;//Times it by kP to get a value scaled with the e sginal
currentError = abs(eValue);
dSignal = abs(((currentError - pastError)/0.005)*kD);
pastError = currentError;
if(pSignal > 0){/**right*/
//printf("right %d\n", pSignal);
set_motor(1, mtrSp);
if(-(mtrSp + 0.5) + pSignal + dSignal <= 0){
set_motor(2, -(mtrSp + 0.5) + pSignal + dSignal);
}else{
set_motor(2, 0);
}
Sleep(0, 5000);
}else if(pSignal < 0){/**left*/
//printf("left %d\n", pSignal);
if(mtrSp + pSignal + dSignal >= 0){
set_motor(1, mtrSp + pSignal + dSignal);/**From a few calculations 40 seems roughly right, max value is 70ish*/
}else{
set_motor(1, 0);
}
set_motor(2, -(mtrSp + 0.5));/**Minuses values if signal is minus it is double negative therefore positive*/
Sleep(0, 5000);
}
}
}
return 0;
}
void MotorController::setRight(int speed) {
//set_motor(2, (int)-(RIGHT_SPEEDS[(int) (speed/1.5) + NUMBER_OF_REVERSE_SPEEDS])); //FOR Dylans code
set_motor(2, -(int) (((RIGHT_SPEEDS[(int) (speed) + NUMBER_OF_REVERSE_SPEEDS])) / 1.5));//for pid
}