Code:
// RA2 steuert RP6 1.11.2010 mic
// Das I2C-Masterdemo6 des m32 angepasst an das arm64-Modul.
// Eine Ergänzung in der Abfrage des Keypads ermöglicht gleichzeitige Bewegungen
// aller Servos und des RP6.
// Auf dem RP6 läuft das unveränderte I2C-Slave-Demo.
// Verbindung zwischen RP6-XBUS Pin8(E_INT), Pin10(SCL), Pin12(SDA) und Pin14(GND)
// und IIC Pin 8-14 auf dem arm64. Beide Boards werden getrennt versorgt.
// PollSwitch16() liefert ein 16bit Abbild der Tasten
// scan_keyboard1() liefert die Tastennummer mit geänderter Portabfrage
// Infos zum Portexpander
// http://www.mikrocontroller.net/artic...pander_PCF8574
#include "RobotArmBaseLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_I2CMasterLib.h"
#define I2C_RP6_BASE_ADR 10 // The default address of the Slave Controller (RP6)
/*****************************************************************************/
// I2C Error handler
/**
* This function gets called automatically if there was an I2C Error like
* the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
* The most common mistakes are:
* - using the wrong address for the slave
* - slave not active or not connected to the I2C-Bus
* - too fast requests for a slower slave
* Be sure to check this if you get I2C errors!
*/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P("\nI2C ERROR - TWI STATE: 0x");
writeInteger(errorState, HEX);
writeChar('\n');
}
/*****************************************************************************/
// Motor function
uint8_t transmit_buffer[10]; // temporary transmit buffer
// A global variable saves space on the heap...
#define CMD_ROTATE 8
#define LEFT 2
#define RIGHT 3
/**
* Rotate function - you can define nearly the same functions as you have on
* the RP6 and just forward the commands with I2C Bus transfers...
* We will make an improved version of this and other functions in another example!
*/
void RP6_rotate(uint8_t desired_speed, uint8_t dir, uint16_t angle)
{
transmit_buffer[0] = 0;
transmit_buffer[1] = CMD_ROTATE;
transmit_buffer[2] = desired_speed;
transmit_buffer[3] = dir;
transmit_buffer[4] = ((angle>>8) & 0xFF);
transmit_buffer[5] = (angle & 0xFF);
I2CTWI_transmitBytes(I2C_RP6_BASE_ADR, transmit_buffer, 6 );
}
#define CMD_MOVE 7
#define FWD 0
#define BWD 1
void RP6_move(uint8_t desired_speed, uint8_t dir, uint16_t ticks)
{
transmit_buffer[0] = 0;
transmit_buffer[1] = CMD_MOVE;
transmit_buffer[2] = desired_speed;
transmit_buffer[3] = dir;
transmit_buffer[4] = ((ticks>>8) & 0xFF);
transmit_buffer[5] = (ticks & 0xFF);
I2CTWI_transmitBytes(I2C_RP6_BASE_ADR, transmit_buffer, 6 );
}
/*****************************************************************************/
// Read all registers function
uint8_t RP6data[32]; // This array will contain all register values of RP6
// after you called readAllRegisters()
// It is better to keep such big arrays GLOBAL as
// they otherwise fill up the stack in memory...
/**
* This function reads ALL registers available in the standard I2C Bus Slave
* Example program for the Robot Base and outputs their values on the serial interface.
* You will see a lot of zeros when the Motors are not running. The main values that are not
* zero are the two Light Sensors and the two free ADC Channels.
*/
void readAllRegisters(void)
{
I2CTWI_transmitByte(I2C_RP6_BASE_ADR, 0); // Start with register 0...
I2CTWI_readBytes(I2C_RP6_BASE_ADR,RP6data, 30); // and read all 30 registers up to
// register Number 29 !
// Now we output the Data we have just read on the serial interface:
writeString_P("\nREADING ALL RP6 REGISTERS:");
uint8_t i = 0;
for(i = 0; i < 30; i++)
{
if(i % 8 == 0) // add some newline chars otherwise everything
writeChar('\n'); // is printed on ONE single line...
else
writeString_P(" | ");
writeChar('#');
writeIntegerLength(i,DEC,2);
writeChar(':');
writeIntegerLength(RP6data[i],DEC,3);
}
writeChar('\n');
}
/**
* Here we demonstrate how to read a few specific registers.
* It is just the same as above, but we only read 4 registers and
* start with register Number 13...
* We also show how to combine values from high and low registers
* back together to a 16 Bit value...
*/
void readLightSensors(void)
{
uint8_t lightSens[4];
I2CTWI_transmitByte(I2C_RP6_BASE_ADR, 13); // Start with register 13 (LSL_L)...
I2CTWI_readBytes(I2C_RP6_BASE_ADR, lightSens, 4); // and read all 4 registers up to
// register Number 16 (LSR_H) !
writeString_P("Light Sensor registers:\n");
writeString_P(" | LSL_L:"); writeInteger(lightSens[0], DEC);
writeString_P(" | LSL_H:"); writeInteger(lightSens[1], DEC);
writeString_P(" | LSR_L:"); writeInteger(lightSens[2], DEC);
writeString_P(" | LSR_H:"); writeInteger(lightSens[3], DEC);
writeString_P("\n\n Light Sensor Values:");
writeString_P(" | LSL:"); writeInteger(lightSens[0] + (lightSens[1]<<8), DEC);
writeString_P(" | LSR:"); writeInteger(lightSens[2] + (lightSens[3]<<8), DEC);
writeChar('\n');
//setCursorPosLCD(1, 3);
//writeIntegerLengthLCD(lightSens[0] + (lightSens[1]<<8), DEC, 4);
//setCursorPosLCD(1, 11);
//writeIntegerLengthLCD(lightSens[2] + (lightSens[3]<<8), DEC, 4);
}
/*****************************************************************************/
// Main function - The program starts here:
int PollSwitch16(void)
{
int i, j, mask=0, x=0;
for(j=0; j<4; j++)
{
DDRA = (1<<j); // PAj auf Ausgang
PORTA = ~(1<<j); // PA7 bis PA4 ist Eingang mit Pullup, PAj ist LOW (Tilde!)
sleep(10); // Warten auf Pegel
x=(PINA>>4); // Eingang PA7-4 wird zu Bit3-0 in x
for(i=0; i<4; i++) // Bits 0-3 überprüfen
if(!(x & (1<<i))) // Wenn Eingang low ist wurde die Taste gedrückt
mask += (1 << (i*4)) * (1<<j); // entsprechendes Bit in Maske setzen
}
PORTA = 0; // Port A deaktivieren
DDRA = 0;
return(mask);
}
int scan_keyboard1(void)
{
int i, mask = PollSwitch16();
for(i=0; i<16; i++) // wurde eine Taste gedrückt?
{
if(mask & (1<<i)) // Taste i gedrückt?
{
if(mask & ~(1<<i))
return(0); // mehrere Tasten gleichzeitig gedrückt!
else
return(i+1); // Tastennummer zurückgeben
}
}
return(0); // keine Taste gedrückt
}
int speed;
void Event( int x )
{
if(x == 0)
{
if(PING & LED_GREEN) PowerLEDoff(); else PowerLEDgreen();
mSleep(300);
}
else
{
PowerLEDred();
//Power_Servos();
if(x & (1<<0)) { Pos_Servo_1-=speed; Power_Servos();}
if(x & (1<<1)) { Pos_Servo_1+=speed; }
if(x & (1<<2)) { Pos_Servo_2+=speed; } // Drehrichtung invers!
if(x & (1<<3)) { Pos_Servo_2-=speed; }
if(x & (1<<4)) { Pos_Servo_3-=speed; }
if(x & (1<<5)) { Pos_Servo_3+=speed; }
if(x & (1<<6)) { Pos_Servo_4-=speed; }
if(x & (1<<7)) { Pos_Servo_4+=speed; }
if(x & (1<<8)) { Pos_Servo_5-=speed; Pos_Servo_6-=speed; }
if(x & (1<<9)) { Pos_Servo_5+=speed; Pos_Servo_6+=speed; }
if(x & (1<<10)) { PowerLEDorange(); speed=5; }
if(x & (1<<11)) { PowerLEDorange(); speed=1; }
if(x & (1<<12)) { RP6_rotate(35,BWD,40); }
if(x & (1<<13)) { RP6_rotate(35,FWD,40); }
if(x & (1<<14)) { RP6_rotate(35,RIGHT,40); }
if(x & (1<<15)) { RP6_rotate(35,LEFT,40); }
}
/*
writeInteger(Pos_Servo_1, 10);
writeString_P(" | ");
writeInteger(Pos_Servo_2, 10);
writeString_P(" | ");
writeInteger(Pos_Servo_3, 10);
writeString_P(" | ");
writeInteger(Pos_Servo_4, 10);
writeString_P(" | ");
writeInteger(Pos_Servo_5, 10);
writeString_P(" | ");
writeInteger(Pos_Servo_6, 10);
writeString_P("\n");
*/
}
int Batterie(void)
{
#define BATTERIE 6
ADMUX = (1 << REFS0) | (1 << REFS1) | BATTERIE; // internal 2.56V reference with external capacitor
ADCSRA |= (1 << ADSC); // Start conversion
while (!(ADCSRA & (1 << ADIF))); // wait for conversion complete
ADCSRA |= (1 << ADIF); // clear ADCIF
return ADCL + (ADCH << 8);
}
void printUBat(uint16_t ubat)
{
writeIntegerLength((((ubat/204.8f)+0.1f)), DEC, 2);
writeChar('.');
writeIntegerLength((((ubat/2.048f)+10)), DEC, 2);
writeChar('V');
}
int main(void)
{
initRobotBase();
PORTA = 0; // Port A deaktivieren
DDRA = 0;
writeString_P("\n\nRA2 arm64 I2C Master Example Program! 3.11.2010 mic\n\n");
int ubat = readADC(ADC_UBAT);
printUBat(ubat);
writeString_P("\n");
// IMPORTANT:
I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
// with 100kHz SCL Frequency
// Register the event handlers:
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
//char counter = 1;
//Start_position(); //Use this function to set the servomotor in the centre.
//This function must be called before using Power_Servos();
speed=1; //Speed for servo from '1'(slow) - '10'(fast!)
Pos_Servo_1=1500;
Pos_Servo_2=1530;
Pos_Servo_3=1100;
Pos_Servo_4=1640;
Pos_Servo_5=580;
Pos_Servo_6=580;
//Power_Servos(); //Use this function to power the servo motors on
while(true)
{
Event(PollSwitch16());
/*
if(scan_keyboard1())
{
writeInteger(scan_keyboard1(), 10);
writeChar('\n');
switch(0)
{
case 1: // Increment a counter and send value to LEDs of the
// Slave Controller:
//setLEDs(0b0001);
//showScreenLCD("INCREMENT", "LED COUNTER");
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, 3, counter);
counter++;
break;
case 2: // Read and display ALL registers of the slave controller:
//setLEDs(0b0010);
//showScreenLCD("READ ALL RP6", "REGISTERS");
readAllRegisters();
break;
case 3: // Read the light sensors and show value on display:
//setLEDs(0b0100);
//showScreenLCD("LIGHT SENSORS:", "L: R:");
readLightSensors();
break;
case 4: // Rotate a very small distance to the left:
//setLEDs(0b1000);
//showScreenLCD("ROTATE A BIT...", "... LEFT!");
RP6_rotate(35,LEFT,40);
break;
case 5: // Rotate a very small distance to the right:
//setLEDs(0b1001);
//showScreenLCD("ROTATE A BIT...", "... RIGHT!");
RP6_rotate(35,RIGHT,40);
break;
}
while(scan_keyboard1());
}
*/
}
return 0;
}
Außerdem mußte ich meine Tastenabfrage ändern, weil das Keypad nicht vollständig bestückt war:
Zitieren
Lesezeichen